french cup + idf cup
authorzer0 <zer0@carbon.local>
Tue, 1 Jun 2010 17:03:07 +0000 (19:03 +0200)
committerzer0 <zer0@carbon.local>
Tue, 1 Jun 2010 17:03:07 +0000 (19:03 +0200)
projects/microb2010/ballboard/actuator.c
projects/microb2010/ballboard/actuator.h
projects/microb2010/ballboard/commands_ballboard.c
projects/microb2010/ballboard/cs.c
projects/microb2010/ballboard/state.c
projects/microb2010/cobboard/actuator.c
projects/microb2010/cobboard/shovel.c
projects/microb2010/cobboard/shovel.h
projects/microb2010/cobboard/state.c
projects/microb2010/mainboard/strat.c
projects/microb2010/mainboard/strat_base.c

index c635fe9..cf9517f 100644 (file)
@@ -48,7 +48,9 @@
 #define ROLLER_REVERSE ROLLER_SPEED
 
 #define FORKROT_DEPLOYED -55000
-#define FORKROT_MID -33000
+#define FORKROT_MID1 -31000
+#define FORKROT_MID2 -27000
+#define FORKROT_EJECT   -12000
 #define FORKROT_PACKED   -4000
 
 void roller_on(void)
@@ -76,9 +78,35 @@ void fork_pack(void)
        cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED);
 }
 
-void fork_mid(void)
+void fork_mid1(void)
 {
-       cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID);
+       cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID1);
+}
+
+void fork_mid2(void)
+{
+       cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID2);
+}
+
+void fork_eject(void)
+{
+       cs_set_consign(&ballboard.forkrot.cs, FORKROT_EJECT);
+}
+
+static uint8_t fork_is_at_pos(int32_t pos)
+{
+       int32_t diff;
+       diff = pos - encoders_spi_get_value(FORKROT_ENCODER);
+       if (diff < 0)
+               diff = -diff;
+       if (diff < 500)
+               return 1;
+       return 0;
+}
+
+uint8_t fork_is_packed(void)
+{
+       return fork_is_at_pos(FORKROT_PACKED);
 }
 
 void actuator_init(void)
index bd93ddc..a8d282b 100644 (file)
@@ -27,6 +27,9 @@ void roller_reverse(void);
 
 void fork_deploy(void);
 void fork_pack(void);
-void fork_mid(void);
+void fork_mid1(void);
+void fork_mid2(void);
+void fork_eject(void);
+uint8_t fork_is_packed(void);
 
 void actuator_init(void);
index bd7920a..95ec614 100644 (file)
@@ -380,13 +380,15 @@ static void cmd_fork_parsed(void *parsed_result,
                fork_deploy();
        else if (!strcmp_P(res->arg1, PSTR("pack")))
                fork_pack();
-       else if (!strcmp_P(res->arg1, PSTR("middle")))
-               fork_mid();
+       else if (!strcmp_P(res->arg1, PSTR("mid1")))
+               fork_mid1();
+       else if (!strcmp_P(res->arg1, PSTR("mid2")))
+               fork_mid2();
 }
 
 prog_char str_fork_arg0[] = "fork";
 parse_pgm_token_string_t cmd_fork_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg0, str_fork_arg0);
-prog_char str_fork_arg1[] = "deploy#pack#middle";
+prog_char str_fork_arg1[] = "deploy#pack#mid1#mid2";
 parse_pgm_token_string_t cmd_fork_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg1, str_fork_arg1);
 
 prog_char help_fork[] = "move fork";
index e1166da..35628d2 100644 (file)
@@ -82,6 +82,7 @@ static void do_cs(void *dummy)
                                         cs_get_filtered_feedback(&ballboard.roller.cs),
                                         cs_get_out(&ballboard.roller.cs));
 
+#if 0
                /* urgent case: stop power on blocking */
                if (ballboard.flags & DO_ERRBLOCKING) {
                        if (bd_get(&ballboard.forktrans.bd) ||
@@ -90,6 +91,7 @@ static void do_cs(void *dummy)
                                ballboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);
                        }
                }
+#endif
        }
        if (ballboard.flags & DO_POWER)
                BRAKE_OFF();
@@ -181,8 +183,8 @@ void microb_cs_init(void)
 
        /* QUADRAMP */
        quadramp_init(&ballboard.forkrot.qr);
-       quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 800); /* set speed */
-       quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */
+       quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 1000, 500); /* set speed */
+       quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 100, 20); /* set accel */
 
        /* CS */
        cs_init(&ballboard.forkrot.cs);
index 6c229a4..edcfb3a 100644 (file)
@@ -166,9 +166,15 @@ static void state_do_eject(void)
                blocked = 0;
 
                while (1) {
+                       /* move fork during ball ejection */
+                       if ((us % 600) < 300)
+                               fork_eject();
+                       else
+                               fork_pack();
 
                        /* no more balls (sensor is heavily filtered) */
-                       if (!sensor_get(S_LOW_BARRIER)) {
+                       if (!sensor_get(S_LOW_BARRIER) &&
+                           !sensor_get(S_HIGH_BARRIER)) {
                                STMCH_DEBUG("%s(): no more balls", __FUNCTION__);
                                break;
                        }
@@ -186,6 +192,7 @@ static void state_do_eject(void)
                                break;
                        }
                }
+               fork_pack();
 
                if (!blocked)
                        break;
@@ -246,8 +253,21 @@ void state_machine(void)
 
                case TAKE_FORK:
                        roller_off();
-                       fork_mid();
-                       time_wait_ms(1300);
+                       fork_mid1();
+                       time_wait_ms(666);
+                       fork_mid2();
+                       time_wait_ms(666);
+                       while (1) {
+                               uint8_t packed;
+
+                               fork_pack();
+                               packed = WAIT_COND_OR_TIMEOUT(fork_is_packed(),
+                                                             500);
+                               if (packed)
+                                       break;
+                               fork_mid2();
+                               time_wait_ms(200);
+                       }
                        state_mode = OFF;
                        break;
 
index 85b2ac1..9d3bdef 100644 (file)
@@ -45,7 +45,7 @@
 #include "main.h"
 #include "actuator.h"
 
-#define COBROLLER_SPEED 666
+#define COBROLLER_SPEED 850
 
 #define SERVO_DOOR_OPEN 300
 #define SERVO_DOOR_CLOSED 525
index 8714367..e06638d 100644 (file)
@@ -46,8 +46,8 @@
 #define SHOVEL_DOWN 100
 #define SHOVEL_MID  4500
 #define SHOVEL_UP   11300
-#define SHOVEL_KICKSTAND_UP   12800
-#define SHOVEL_KICKSTAND_DOWN 10000
+#define SHOVEL_KICKSTAND_UP   13400
+#define SHOVEL_KICKSTAND_DOWN 10400
 
 static int32_t shovel_k1 = 1000;
 static int32_t shovel_k2 = 20;
@@ -140,9 +140,9 @@ void shovel_up(void)
 {
        shovel_current_limit_enable(0);
        if (state_get_cob_count() <= 1)
-               quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 2500);
+               quadramp_set_1st_order_vars(&cobboard.shovel.qr, 700, 2500);
        else
-               quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2000, 2500);
+               quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 2500);
        quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 80, 15);
        cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP);
 }
@@ -175,6 +175,11 @@ uint8_t shovel_is_down(void)
        return shovel_is_at_pos(SHOVEL_DOWN);
 }
 
+uint8_t shovel_is_mid(void)
+{
+       return shovel_is_at_pos(SHOVEL_MID);
+}
+
 void shovel_init(void)
 {
        shovel_autopos();
index 0019d2d..0a61102 100644 (file)
@@ -37,5 +37,6 @@ void shovel_kickstand_down(void);
 
 uint8_t shovel_is_up(void);
 uint8_t shovel_is_down(void);
+uint8_t shovel_is_mid(void);
 
 #endif /* _SHOVEL_H_ */
index 535829f..9c6af8c 100644 (file)
@@ -70,6 +70,7 @@ static uint8_t cob_count;
 #define KICKSTAND_DOWN(mode)  ((mode) == I2C_COBBOARD_MODE_KICKSTAND_DOWN)
 
 uint8_t state_debug = 0;
+static uint8_t state_cob_partial = 0;
 
 uint8_t state_get_cob_count(void)
 {
@@ -98,6 +99,15 @@ static uint8_t state_no_cob_inside(void)
                !sensor_get(S_COB_INSIDE_R);
 }
 
+static uint8_t state_cob_inside_enhanced(void)
+{
+       if (sensor_get(S_COB_INSIDE_L))
+               state_cob_partial = 1;
+       if (sensor_get(S_COB_INSIDE_R))
+               state_cob_partial = 1;
+       return state_cob_inside();
+}
+
 static uint8_t state_spicklemode_deployed(uint8_t side)
 {
        if (side == I2C_LEFT_SIDE)
@@ -214,6 +224,8 @@ uint8_t state_get_status(void)
 /* harvest cobs from area */
 static void state_do_harvest(uint8_t side)
 {
+       uint8_t i = 0;
+
        /* if there is no cob, return */
        if (cob_falling_edge(side) == 0)
                return;
@@ -232,11 +244,15 @@ static void state_do_harvest(uint8_t side)
        cobroller_on(side);
 
        /* check that cob is correctly in place */
-       if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) {
-               if (state_no_cob_inside()) {
+       state_cob_partial = 0;
+       if (WAIT_COND_OR_TIMEOUT(state_cob_inside_enhanced(), 750) == 0) {
+               if (state_no_cob_inside() && state_cob_partial == 0) {
                        STMCH_DEBUG("no cob");
                        return;
                }
+               else if (state_no_cob_inside() && state_cob_partial == 1)
+                       goto cont;
+
                STMCH_DEBUG("bad cob state");
 
                /* while cob is not correctly placed try to extract
@@ -256,6 +272,9 @@ static void state_do_harvest(uint8_t side)
                        time_wait_ms(250);
                        shovel_down();
                        time_wait_ms(250);
+
+                       if (EJECT(state_mode))
+                               return;
                }
 
                STMCH_DEBUG("cob removed");
@@ -265,6 +284,7 @@ static void state_do_harvest(uint8_t side)
                }
        }
 
+ cont:
        /* cob is inside, switch off roller */
        cobroller_off(side);
        cob_count ++;
@@ -289,14 +309,40 @@ static void state_do_harvest(uint8_t side)
        /* store it */
        shovel_up();
 
+       i = 0;
        while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 600) == 0) {
                STMCH_DEBUG("shovel blocked");
                shovel_down();
-               time_wait_ms(250);
-               shovel_up();
+
+               if (i == 4)
+                       break;
+
                /* if eject command is received, force exit */
                if (EJECT(state_mode))
                        return;
+
+               time_wait_ms(250);
+               shovel_up();
+               i ++;
+       }
+
+       /* bad state, try to eject to cobs */
+       if (i == 4) {
+               servo_door_open();
+               shovel_mid();
+
+               while (WAIT_COND_OR_TIMEOUT(shovel_is_mid(), 600) == 0) {
+                       STMCH_DEBUG("ejecting cobs");
+
+                       shovel_down();
+                       time_wait_ms(250);
+                       if (state_no_cob_inside()) {
+                               servo_door_close();
+                               cob_count = 0;
+                               return;
+                       }
+                       shovel_mid();
+               }
        }
 
        state_debug_wait_key_pressed();
index 41e7e8b..6090ca8 100644 (file)
@@ -223,6 +223,20 @@ static void check_tomato(void)
 #endif
 }
 
+static void remove_cob(uint8_t idx)
+{
+       if (strat_db.corn_table[idx]->corn.color == I2C_COB_BLACK)
+               return;
+
+       if (strat_db.corn_table[idx]->time_removed == -1) {
+               strat_db.corn_table[idx]->time_removed = time_get_s();
+#ifdef HOST_VERSION
+               cobboard.cob_count ++;
+               printf("add cob %d\n", idx);
+#endif
+       }
+}
+
 /* mark corn as not present and give correct commands to the cobboard
  * for spickles */
 static void check_corn(void)
@@ -274,7 +288,7 @@ static void check_corn(void)
                              lcob == I2C_COB_WHITE ? "white" : "black", lidx);
                corn_set_color(strat_db.corn_table[lidx], lcob);
        }
-       if (!__y_is_more_than(l_yspickle, 600))
+       if (cur_time > 5 && !__y_is_more_than(l_yspickle, 600))
                l_y_too_high_pack = 1;
 
        /* detect cob on right side */
@@ -286,37 +300,38 @@ static void check_corn(void)
                              rcob == I2C_COB_WHITE ? "white" : "black", ridx);
                corn_set_color(strat_db.corn_table[ridx], rcob);
        }
-       if (!__y_is_more_than(r_yspickle, 600))
+       if (cur_time > 5 && !__y_is_more_than(r_yspickle, 600))
                r_y_too_high_pack = 1;
 
+       /* re-enable white cob */
+       if (lcob == I2C_COB_WHITE && lcob_near &&
+           strat_db.corn_table[lidx]->present == 0) {
+               strat_db.corn_table[lidx]->present = 1;
+               strat_db.corn_table[lidx]->time_removed = -1;
+       }
+       if (rcob == I2C_COB_WHITE && rcob_near &&
+           strat_db.corn_table[ridx]->present == 0) {
+               strat_db.corn_table[ridx]->present = 1;
+               strat_db.corn_table[ridx]->time_removed = -1;
+       }
+
        /* control the cobboard mode for left spickle */
        need_lpack = get_cob_count() >= 5 || strat_want_pack ||
                strat_lpack60 || strat_opponent_lpack || l_y_too_high_pack;
 
-       if (lcob_near && strat_db.corn_table[lidx]->present) {
+       if (lcob_near &&
+           (strat_db.corn_table[lidx]->present ||
+            strat_db.corn_table[lidx]->time_removed == -1)) {
                if (need_lpack) {
                        /* nothing  */
                }
                else {
                        /* deploy spickle and harvest white ones */
-                       if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) {
+                       if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
                                i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE);
-                               if (strat_db.corn_table[lidx]->time_removed == -1
-#ifndef HOST_VERSION
-                                   && cobboard.status == I2C_COBBOARD_STATUS_LBUSY
-#endif
-                                   ) {
-                                       strat_db.corn_table[lidx]->time_removed = time_get_s();
-#ifdef HOST_VERSION
-                                       cobboard.cob_count ++;
-                                       printf("add cob %d\n", lidx);
-#else
-                                       strat_db.corn_table[lidx]->present = 0;
-#endif
-                               }
-                       }
                        else
                                i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
+                       remove_cob(lidx);
                }
        }
        else {
@@ -330,30 +345,19 @@ static void check_corn(void)
        /* control the cobboard mode for right spickle */
        need_rpack = get_cob_count() >= 5 || strat_want_pack ||
                strat_rpack60 || strat_opponent_rpack || r_y_too_high_pack;
-       if (rcob_near && strat_db.corn_table[ridx]->present) {
+       if (rcob_near &&
+           (strat_db.corn_table[ridx]->present ||
+            strat_db.corn_table[ridx]->time_removed == -1)) {
                if (need_rpack) {
                        /* nothing */
                }
                else {
                        /* deploy spickle and harvest white ones */
-                       if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) {
+                       if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE)
                                i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE);
-                               if (strat_db.corn_table[ridx]->time_removed == -1
-#ifndef HOST_VERSION
-                                   && cobboard.status == I2C_COBBOARD_STATUS_RBUSY
-#endif
-                                   ) {
-                                       strat_db.corn_table[ridx]->time_removed = time_get_s();
-#ifdef HOST_VERSION
-                                       cobboard.cob_count ++;
-                                       printf("add cob %d\n", ridx);
-#else
-                                       strat_db.corn_table[ridx]->present = 0;
-#endif
-                               }
-                       }
                        else
                                i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
+                       remove_cob(ridx);
                }
        }
        else {
@@ -522,8 +526,6 @@ static uint8_t strat_eject(void)
 #ifdef HOST_VERSION
                time_wait_ms(2000);
 #else
-               WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_BUSY,
-                                    2000);
                WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_READY,
                                     2000);
 #endif
@@ -533,7 +535,9 @@ static uint8_t strat_eject(void)
                time_wait_ms(300);
        }
 
-       if (get_cob_count() > 0) {
+       if (get_cob_count() > 0 ||
+           cobboard.status == I2C_COBBOARD_STATUS_LBUSY ||
+           cobboard.status == I2C_COBBOARD_STATUS_RBUSY) {
                /* half turn */
                trajectory_a_abs(&mainboard.traj, COLOR_A(-110));
                err = wait_traj_end(END_INTR|END_TRAJ);
@@ -574,7 +578,8 @@ static uint8_t strat_beginning(uint8_t do_initturn)
 
        if (do_initturn) {
                //strat_set_speed(600, 60);
-               strat_set_speed(450, 50);
+               //strat_set_speed(450, 50);
+               strat_set_speed(350, 40);
                trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
                err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
                                            TRAJ_FLAGS_STD);
@@ -919,15 +924,15 @@ uint8_t run_to_the_hills(uint8_t orange_color)
        strat_hardstop();
        i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
 
-       time_wait_ms(1100);
+       time_wait_ms(500);
 
        trajectory_d_rel(&mainboard.traj, 15);
-       time_wait_ms(400);
+       time_wait_ms(700);
        strat_hardstop();
        time_wait_ms(200);
 
        /* reach top, go down */
-       trajectory_d_rel(&mainboard.traj, -HILL_LEN);
+       trajectory_d_a_rel(&mainboard.traj, -HILL_LEN, -2);
 
        if (orange_color == our_color)
                err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
@@ -949,10 +954,18 @@ uint8_t run_to_the_hills(uint8_t orange_color)
                                            TRAJ_FLAGS_SMALL_DIST);
 
        DEBUG(E_USER_STRAT, "deploy support balls");
-       strat_set_acc(ad, aa);
-       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
        support_balls_deploy();
 
+       if (orange_color == I2C_COLOR_YELLOW) {
+               strat_set_acc(ad, aa);
+               strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+       }
+       else {
+               strat_set_acc(ad, 0.4);
+               strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+               trajectory_d_a_rel(&mainboard.traj, -500, 20);
+       }
+
        /* wait to be near the wall */
        if (orange_color == our_color)
                err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300,
@@ -962,6 +975,9 @@ uint8_t run_to_the_hills(uint8_t orange_color)
                                            AREA_X - 300,
                                            TRAJ_FLAGS_SMALL_DIST);
 
+       strat_set_acc(ad, aa);
+       strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
        /* restore BD coefs */
        bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
        bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
@@ -984,6 +1000,7 @@ uint8_t run_to_the_hills(uint8_t orange_color)
 
        trajectory_d_rel(&mainboard.traj, -250);
        err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+#if 0
        if (orange_color == our_color)
                trajectory_a_abs(&mainboard.traj, 180);
        else
@@ -1005,7 +1022,7 @@ uint8_t run_to_the_hills(uint8_t orange_color)
 
        trajectory_d_rel(&mainboard.traj, -250);
        err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
+#endif
        /* revert acceleration and speed */
        pid_set_gains(&mainboard.angle.pid, p, i, d);
        pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
index 5a819c9..90762a6 100644 (file)
@@ -456,8 +456,8 @@ uint8_t strat_obstacle(void)
        }
 
        /* opponent is in front of us */
-       if ((mainboard.speed_d >= 0 && opp_d < 500 && (opp_a > 315 || opp_a < 45)) ||
-           (mainboard.speed_d >= 0 && opp_d < 650 && (opp_a > 330 || opp_a < 30))) {
+       if ((mainboard.speed_d >= 0 && opp_d < 600 && (opp_a > 315 || opp_a < 45)) ||
+           (mainboard.speed_d >= 0 && opp_d < 800 && (opp_a > 330 || opp_a < 30))) {
                DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
                      "xrel=%d yrel=%d (speed_d=%d)",
                      opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
@@ -465,8 +465,8 @@ uint8_t strat_obstacle(void)
                return 1;
        }
        /* opponent is behind us */
-       if ((mainboard.speed_d < 0 && opp_d < 500 && (opp_a < 225 && opp_a > 135)) ||
-           (mainboard.speed_d < 0 && opp_d < 650 && (opp_a < 210 && opp_a > 150))) {
+       if ((mainboard.speed_d < 0 && opp_d < 600 && (opp_a < 225 && opp_a > 135)) ||
+           (mainboard.speed_d < 0 && opp_d < 800 && (opp_a < 210 && opp_a > 150))) {
                DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
                      opp_d, opp_a, x_rel, y_rel);
                sensor_obstacle_disable();