From 78150017ab8c5615af414df706a0525fe7c262ae Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 17 Apr 2010 11:46:41 +0200 Subject: [PATCH] avant la coupe de belgique --- .../trajectory_manager_core.c | 2 +- projects/microb2010/ballboard/main.c | 3 + projects/microb2010/ballboard/state.c | 6 +- projects/microb2010/cobboard/spickle.c | 8 +- projects/microb2010/cobboard/state.c | 2 +- .../microb2010/mainboard/commands_mainboard.c | 225 ++---------------- projects/microb2010/mainboard/commands_traj.c | 12 +- projects/microb2010/mainboard/cs.c | 12 +- projects/microb2010/mainboard/display.py | 38 ++- projects/microb2010/mainboard/main.c | 4 +- projects/microb2010/mainboard/main.h | 6 +- projects/microb2010/mainboard/robotsim.c | 11 +- projects/microb2010/mainboard/strat.c | 35 ++- projects/microb2010/mainboard/strat_base.c | 43 ++-- projects/microb2010/mainboard/strat_corn.c | 214 +++++++++++++++++ projects/microb2010/mainboard/strat_corn.h | 18 +- 16 files changed, 382 insertions(+), 257 deletions(-) diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index c6c70fc..18f5c90 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -1007,10 +1007,10 @@ static void start_clitoid(struct trajectory *traj) delete_event(traj); DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f", __FUNCTION__, Va, Aa); - traj->state = RUNNING_CLITOID_CURVE; d = fabs(R_mm * a_rad); d *= 3.; /* margin to avoid deceleration */ trajectory_d_a_rel(traj, d, DEG(a_rad)); + traj->state = RUNNING_CLITOID_CURVE; set_quadramp_acc(traj, traj->d_acc, Aa); set_quadramp_speed(traj, traj->d_speed, Va); } diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c index ed8716b..de8bc0c 100755 --- a/projects/microb2010/ballboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -58,6 +58,7 @@ #include "actuator.h" #include "cs.h" #include "i2c_protocol.h" +#include "state.h" /* 0 means "programmed" * ---- with 16 Mhz quartz @@ -252,6 +253,8 @@ int main(void) printf_P(PSTR("\r\n")); printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n")); + + state_machine(); cmdline_interact(); return 0; diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c index 0492e18..57d44ff 100644 --- a/projects/microb2010/ballboard/state.c +++ b/projects/microb2010/ballboard/state.c @@ -56,7 +56,7 @@ static struct vt100 local_vt100; static volatile uint8_t state_mode; -static uint8_t ball_count; +static volatile uint8_t ball_count; /* short aliases */ #define INIT I2C_BALLBOARD_MODE_INIT @@ -82,6 +82,7 @@ uint8_t state_get_ball_count(void) return ball_count; } +#if 0 static void state_debug_wait_key_pressed(void) { if (!state_debug) @@ -89,6 +90,7 @@ static void state_debug_wait_key_pressed(void) printf_P(PSTR("press a key\r\n")); while (!cmdline_keypressed()); } +#endif /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode) @@ -128,7 +130,7 @@ uint8_t state_get_mode(void) /* harvest balls from area */ static void state_do_harvest(void) { - state_debug_wait_key_pressed(); + //state_debug_wait_key_pressed(); roller_on(); } diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index dcee125..cbcb65c 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -67,16 +67,16 @@ static struct spickle_params spickle = { &cobboard.right_spickle, }, .pos_deployed = { - 0, /* left */ - 0, /* right */ + 200, /* left */ + 200, /* right */ }, .pos_mid = { 25000, /* left */ 26000, /* right */ }, .pos_packed = { - 53000, /* left */ - 54500, /* right */ + 55800, /* left */ + 55800, /* right */ }, }; diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 389411e..d310168 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -233,7 +233,7 @@ static void state_do_harvest(uint8_t side) /* store it */ shovel_up(); - while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) { + while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 600) == 0) { STMCH_DEBUG("shovel blocked"); shovel_down(); time_wait_ms(250); diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 7c42411..de73518 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -296,9 +296,6 @@ struct cmd_start_result { /* function called when cmd_start is parsed successfully */ static void cmd_start_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_start_result *res = parsed_result; uint8_t old_level = gen.log_level; @@ -327,7 +324,6 @@ static void cmd_start_parsed(void *parsed_result, void *data) gen.logs[NB_LOGS] = 0; gen.log_level = old_level; -#endif } prog_char str_start_arg0[] = "start"; @@ -1130,65 +1126,6 @@ struct cmd_test_result { int32_t dist; }; -#define LINE_UP 0 -#define LINE_DOWN 1 -#define LINE_R_UP 2 -#define LINE_L_DOWN 3 -#define LINE_L_UP 4 -#define LINE_R_DOWN 5 - -struct line_2pts { - point_t p1; - point_t p2; -}; - -static void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) -{ - float n = num; - - switch (dir) { - - case LINE_UP: - l->p1.x = n * 450 + 375; - l->p1.y = COLOR_Y(0); - l->p2.x = n * 450 + 375; - l->p2.y = COLOR_Y(2100); - break; - case LINE_DOWN: - l->p1.x = n * 450 + 375; - l->p1.y = COLOR_Y(2100); - l->p2.x = n * 450 + 375; - l->p2.y = COLOR_Y(0); - break; - case LINE_R_UP: - l->p1.x = 150; - l->p1.y = COLOR_Y(-n * 500 + 1472); - l->p2.x = 2850; - l->p2.y = COLOR_Y((-n + 4) * 500 + 972); - break; - case LINE_L_DOWN: - l->p1.x = 2850; - l->p1.y = COLOR_Y((-n + 4) * 500 + 972); - l->p2.x = 150; - l->p2.y = COLOR_Y(-n * 500 + 1472); - break; - case LINE_L_UP: - l->p1.x = 2850; - l->p1.y = COLOR_Y(-n * 500 + 1472); - l->p2.x = 150; - l->p2.y = COLOR_Y((-n + 4) * 500 + 972); - break; - case LINE_R_DOWN: - l->p1.x = 150; - l->p1.y = COLOR_Y((-n + 4) * 500 + 972); - l->p2.x = 2850; - l->p2.y = COLOR_Y(-n * 500 + 1472); - break; - default: - break; - } -} - #if 0 static void reverse_line(struct line_2pts *l) { @@ -1203,142 +1140,14 @@ static void reverse_line(struct line_2pts *l) } #endif -/* return 1 if there is a corn near, and fill the index ptr */ -static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) -{ -#define SENSOR_CORN_DIST 225 -#define SENSOR_CORN_ANGLE 90 - double x = position_get_x_double(&mainboard.pos); - double y = position_get_y_double(&mainboard.pos); - double a_rad = position_get_a_rad_double(&mainboard.pos); - double x_corn, y_corn; - int16_t x_corn_int, y_corn_int; - - if (side == I2C_LEFT_SIDE) { - x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; - y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; - } - else { - x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; - y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; - } - x_corn_int = x_corn; - y_corn_int = y_corn; - - *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); - if (*corn_idx < 0) - return 0; - return 1; -} - -/* - * - send the correct commands to the spickles - * - return 1 if we need to stop (cobboard is stucked) -*/ -static uint8_t handle_spickles(void) -{ - int8_t corn_idx; - - if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE)) - i2c_cobboard_mode_deploy(I2C_LEFT_SIDE); - else { - if (corn_table[corn_idx] == TYPE_WHITE_CORN) - i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); - else - i2c_cobboard_mode_pack(I2C_LEFT_SIDE); - } -/* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */ -/* time_wait_ms(100); */ - - if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE)) - i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE); - else { - if (corn_table[corn_idx] == TYPE_WHITE_CORN) - i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); - else - i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); - } - - return 0; -} - -static void line2line(uint8_t dir1, uint8_t num1, - uint8_t dir2, uint8_t num2) -{ - double line1_a_rad, line1_a_deg, line2_a_rad; - double diff_a_deg, diff_a_deg_abs, beta_deg; - double radius; - struct line_2pts l1, l2; - line_t ll1, ll2; - point_t p; - uint8_t err; - - /* convert to 2 points */ - num2line(&l1, dir1, num1); - num2line(&l2, dir2, num2); - - printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); - printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); - - /* convert to line eq and find intersection */ - pts2line(&l1.p1, &l1.p2, &ll1); - pts2line(&l2.p1, &l2.p2, &ll2); - intersect_line(&ll1, &ll2, &p); - - line1_a_rad = atan2(l1.p2.y - l1.p1.y, - l1.p2.x - l1.p1.x); - line1_a_deg = DEG(line1_a_rad); - line2_a_rad = atan2(l2.p2.y - l2.p1.y, - l2.p2.x - l2.p1.x); - diff_a_deg = DEG(line2_a_rad - line1_a_rad); - diff_a_deg_abs = fabs(diff_a_deg); - - if (diff_a_deg_abs < 70.) { - radius = 200; - if (diff_a_deg > 0) - beta_deg = 40; - else - beta_deg = -40; - } - else if (diff_a_deg_abs < 100.) { - radius = 100; - if (diff_a_deg > 0) - beta_deg = 40; - else - beta_deg = -40; - } - else { - radius = 120; - if (diff_a_deg > 0) - beta_deg = 60; - else - beta_deg = -60; - } - - trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, - line1_a_deg, 150., diff_a_deg, beta_deg, - radius, xy_norm(l1.p1.x, l1.p1.y, - p.x, p.y)); - err = 0; - while (err == 0) { - err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); - if (err == 0) { - /* cobboard is stucked */ - trajectory_hardstop(&mainboard.traj); - return; /* XXX do something */ - } - err = test_traj_end(0xFF); - } - return; -} - /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { + uint8_t err; + #ifdef HOST_VERSION - strat_reset_pos(298.48, 309.21, 70.02); + strat_reset_pos(298.48, COLOR_Y(309.21), + COLOR_A(70.02)); mainboard.angle.on = 1; mainboard.distance.on = 1; strat_set_speed(250, SPEED_ANGLE_FAST); @@ -1346,11 +1155,29 @@ static void cmd_test_parsed(void *parsed_result, void *data) init_corn_table(0, 0); time_wait_ms(100); - line2line(LINE_UP, 0, LINE_R_DOWN, 2); - line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); - line2line(LINE_R_UP, 2, LINE_UP, 5); - + err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); + err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); + err = line2line(LINE_R_UP, 2, LINE_UP, 5); trajectory_hardstop(&mainboard.traj); + + /* ball ejection */ + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + time_wait_ms(2000); + + /* half turn */ + trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); + err = wait_traj_end(END_INTR|END_TRAJ); + i2c_cobboard_mode_pack(I2C_LEFT_SIDE); + i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + trajectory_a_rel(&mainboard.traj, COLOR_A(180)); + err = wait_traj_end(END_INTR|END_TRAJ); + + /* cob ejection */ + trajectory_d_rel(&mainboard.traj, -100); + err = wait_traj_end(END_INTR|END_TRAJ); + i2c_cobboard_mode_eject(); + time_wait_ms(2000); } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 8c59996..be9150b 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -766,14 +766,13 @@ static void auto_position(void) strat_get_speed(&old_spdd, &old_spda); strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST); - trajectory_d_rel(&mainboard.traj, 300); - err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; - wait_ms(100); - strat_reset_pos(ROBOT_WIDTH/2, + strat_reset_pos(ROBOT_WIDTH/2 + 100, COLOR_Y(ROBOT_HALF_LENGTH_FRONT), COLOR_A(-90)); + strat_hardstop(); trajectory_d_rel(&mainboard.traj, -180); err = wait_traj_end(END_INTR|END_TRAJ); @@ -785,14 +784,13 @@ static void auto_position(void) if (err == END_INTR) goto intr; - trajectory_d_rel(&mainboard.traj, 300); - err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; - wait_ms(100); strat_reset_pos(ROBOT_HALF_LENGTH_FRONT, DO_NOT_SET_POS, 180); + strat_hardstop(); trajectory_d_rel(&mainboard.traj, -170); err = wait_traj_end(END_INTR|END_TRAJ); diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index b8d6e08..516f00b 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -213,9 +213,9 @@ void microb_cs_init(void) RIGHT_ENCODER, IMP_COEF * 1.); #else rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, - LEFT_ENCODER, IMP_COEF * -1.036); + LEFT_ENCODER, IMP_COEF * -1.011718); rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, - RIGHT_ENCODER, IMP_COEF * 1.037); + RIGHT_ENCODER, IMP_COEF * 1.012695); #endif /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); @@ -261,7 +261,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&mainboard.angle.bd); bd_set_speed_threshold(&mainboard.angle.bd, 80); - bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50); + bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 20); /* ---- CS distance */ /* PID */ @@ -287,7 +287,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&mainboard.distance.bd); bd_set_speed_threshold(&mainboard.distance.bd, 60); - bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50); + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20); #ifndef HOST_VERSION /* ---- CS left_cobroller */ @@ -332,8 +332,8 @@ void microb_cs_init(void) #endif /* !HOST_VERSION */ /* set them on !! */ - mainboard.angle.on = 0; - mainboard.distance.on = 0; + mainboard.angle.on = 1; + mainboard.distance.on = 1; mainboard.left_cobroller.on = 1; mainboard.right_cobroller.on = 1; diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index eb63f03..6106ff0 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -41,6 +41,10 @@ greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ] greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea) greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3)) +YELLOW = 0 +BLUE = 1 +color = YELLOW + def square(sz): sq = curve() sq.pos = [(-sz, -sz, 0.3), @@ -245,33 +249,49 @@ def toggle_obj_disp(): else: o.visible = 1 +def toggle_color(): + global color + global BLUE, YELLOW + if color == YELLOW: + color = BLUE + else: + color = YELLOW def set_robot(): global robot, last_pos, robot_trail, robot_trail_list global save_pos, robot_x, robot_y, robot_a - axis = (math.cos(robot_a*math.pi/180), - math.sin(robot_a*math.pi/180), + if color == YELLOW: + tmp_x = robot_x - AREA_X/2 + tmp_y = robot_y - AREA_Y/2 + tmp_a = robot_a + else: + tmp_x = -robot_x + AREA_X/2 + tmp_y = -robot_y + AREA_Y/2 + tmp_a = robot_a + + robot.pos = (tmp_x, tmp_y, ROBOT_HEIGHT/2) + axis = (math.cos(tmp_a*math.pi/180), + math.sin(tmp_a*math.pi/180), 0) - robot.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2) robot.axis = axis robot.size = (250, 320, ROBOT_HEIGHT) - lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a+90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a+90)*math.pi/180), + lspickle.pos = (tmp_x + (robot_lspickle*60) * math.cos((tmp_a+90)*math.pi/180), + tmp_y + (robot_lspickle*60) * math.sin((tmp_a+90)*math.pi/180), ROBOT_HEIGHT/2) lspickle.axis = axis lspickle.size = (20, 320, 5) - rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a-90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a-90)*math.pi/180), + rspickle.pos = (tmp_x + (robot_rspickle*60) * math.cos((tmp_a-90)*math.pi/180), + tmp_y + (robot_rspickle*60) * math.sin((tmp_a-90)*math.pi/180), ROBOT_HEIGHT/2) rspickle.axis = axis rspickle.size = (20, 320, 5) # save position - save_pos.append((robot.pos.x, robot.pos, robot_a)) + save_pos.append((robot.pos.x, robot.pos.y, tmp_a)) pos = robot.pos.x, robot.pos.y, 0.3 if pos != last_pos: @@ -372,6 +392,8 @@ while True: save() elif k == "h": toggle_obj_disp() + elif k == "i": + toggle_color() # EOF if l == "": diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 98d3749..633a35d 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -83,8 +83,8 @@ struct genboard gen; struct mainboard mainboard; -struct cobboard cobboard; -struct ballboard ballboard; +volatile struct cobboard cobboard; +volatile struct ballboard ballboard; #ifndef HOST_VERSION /***********************/ diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 21c0065..367718f 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -201,15 +201,15 @@ struct cobboard { /* state of ballboard, synchronized through i2c */ struct ballboard { - uint8_t mode; + volatile uint8_t mode; uint8_t status; uint8_t ball_count; }; extern struct genboard gen; extern struct mainboard mainboard; -extern struct cobboard cobboard; -extern struct ballboard ballboard; +extern volatile struct cobboard cobboard; +extern volatile struct ballboard ballboard; /* start the bootloader */ void bootloader(void); diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index a0fa5a0..db161b8 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -67,11 +67,16 @@ void robotsim_dump(void) { char buf[BUFSIZ]; int len; + int16_t x, y, a; + + x = position_get_x_s16(&mainboard.pos); + y = position_get_y_s16(&mainboard.pos); + a = position_get_a_deg_s16(&mainboard.pos); +/* y = COLOR_Y(y); */ +/* a = COLOR_A(a); */ len = snprintf(buf, sizeof(buf), "pos=%d,%d,%d\n", - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); + x, y, a); hostsim_lock(); write(fdw, buf, len); hostsim_unlock(); diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 1a525e5..c253e5f 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -112,7 +112,7 @@ void strat_preinit(void) mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | DO_POS | DO_BD | DO_POWER; - i2c_cobboard_mode_init(); + //i2c_cobboard_mode_init(); strat_dump_conf(); strat_dump_infos(__FUNCTION__); } @@ -155,6 +155,10 @@ void strat_init(void) time_reset(); interrupt_traj_reset(); + i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); + i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); + /* used in strat_base for END_TIMER */ mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | DO_POS | DO_BD | DO_TIMER | DO_POWER; @@ -197,6 +201,35 @@ void strat_event(void *dummy) static uint8_t strat_beginning(void) { + uint8_t err; + + strat_set_speed(250, SPEED_ANGLE_FAST); + //init_corn_table(0, 0); + + err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); + err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); + err = line2line(LINE_R_UP, 2, LINE_UP, 5); + trajectory_hardstop(&mainboard.traj); + + /* ball ejection */ + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + time_wait_ms(2000); + + /* half turn */ + trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); + err = wait_traj_end(END_INTR|END_TRAJ); + i2c_cobboard_mode_pack(I2C_LEFT_SIDE); + i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + trajectory_a_rel(&mainboard.traj, COLOR_A(180)); + err = wait_traj_end(END_INTR|END_TRAJ); + + /* cob ejection */ + trajectory_d_rel(&mainboard.traj, -100); + err = wait_traj_end(END_INTR|END_TRAJ); + i2c_cobboard_mode_eject(); + time_wait_ms(2000); + return END_TRAJ; } diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index 2c52778..a6724ad 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology (2009) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -101,7 +101,7 @@ const char *get_err(uint8_t err) return "END_UNKNOWN"; } -void strat_hardstop(void) +void strat_hardstop(void) { trajectory_hardstop(&mainboard.traj); pid_reset(&mainboard.angle.pid); @@ -140,7 +140,7 @@ uint8_t strat_goto_xy_force(int16_t x, int16_t y) return END_BLOCKING; } -/* reset position */ +/* reset position */ void strat_reset_pos(int16_t x, int16_t y, int16_t a) { int16_t posx = position_get_x_s16(&mainboard.pos); @@ -162,7 +162,7 @@ void strat_reset_pos(int16_t x, int16_t y, int16_t a) DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__); } -/* +/* * decrease gain on angle PID, and go forward until we reach the * border. */ @@ -171,12 +171,17 @@ uint8_t strat_calib(int16_t dist, uint8_t flags) int32_t p = pid_get_gain_P(&mainboard.angle.pid); int32_t i = pid_get_gain_I(&mainboard.angle.pid); int32_t d = pid_get_gain_D(&mainboard.angle.pid); + int32_t max_in = pid_get_max_in(&mainboard.angle.pid); + int32_t max_i = pid_get_max_I(&mainboard.angle.pid); + int32_t max_out = pid_get_max_out(&mainboard.angle.pid); uint8_t err; - pid_set_gains(&mainboard.angle.pid, 150, 0, 2000); + pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000); + pid_set_gains(&mainboard.angle.pid, 200, 0, 2000); trajectory_d_rel(&mainboard.traj, dist); err = wait_traj_end(flags); pid_set_gains(&mainboard.angle.pid, p, i, d); + pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out); return err; } @@ -190,7 +195,7 @@ static void strat_update_traj_speed(void) a = strat_speed_a; if (strat_limit_speed_a && a > strat_limit_speed_a) a = strat_limit_speed_a; - + trajectory_set_speed(&mainboard.traj, d, a); } @@ -272,7 +277,7 @@ void strat_limit_speed(void) /* start the strat */ void strat_start(void) -{ +{ int8_t i; uint8_t err; @@ -294,7 +299,7 @@ void strat_start(void) break; } } - + /* if start sw plugged */ if (!sensor_get(S_START_SWITCH)) { printf_P(PSTR("Ready, unplug start switch to start\r\n")); @@ -344,14 +349,14 @@ uint8_t strat_obstacle(void) /* opponent is in front of us */ if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) { DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d " - "xrel=%d yrel=%d (speed_d=%d)", + "xrel=%d yrel=%d (speed_d=%d)", opp_d, opp_a, x_rel, y_rel, mainboard.speed_d); sensor_obstacle_disable(); return 1; } /* opponent is behind us */ if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) { - DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", + 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(); return 1; @@ -371,7 +376,7 @@ void interrupt_traj_reset(void) } uint8_t test_traj_end(uint8_t why) -{ +{ uint16_t cur_timer; point_t robot_pt; @@ -389,26 +394,26 @@ uint8_t test_traj_end(uint8_t why) } if ((why & END_INTR) && traj_intr) { - interrupt_traj_reset(); + interrupt_traj_reset(); return END_INTR; } if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj)) return END_TRAJ; - + /* we are near the destination point (depends on current * speed) AND the robot is in the area bounding box. */ if (why & END_NEAR) { - int16_t d_near = 100; - + int16_t d_near = 100; + if (mainboard.speed_d > 2000) d_near = 150; - + if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) && is_in_boundingbox(&robot_pt)) return END_NEAR; } - + if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) { strat_hardstop(); return END_BLOCKING; @@ -443,7 +448,7 @@ uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line) line, opp_x, opp_y, opp_d, opp_a); } else { - DEBUG(E_USER_STRAT, "Got %s at line %d", + DEBUG(E_USER_STRAT, "Got %s at line %d", get_err(ret), line); } return ret; diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 72029b3..4262e34 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -29,7 +29,40 @@ #include #include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" +#include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_corn.h" +#include "strat_utils.h" +#include "sensor.h" +#include "actuator.h" /* XXX TODO static @@ -525,6 +558,187 @@ int get_path(struct djpoint *start, struct djpoint *end) return 0; /* XXX */ } +/* return 1 if there is a corn near, and fill the index ptr */ +static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) +{ +#define SENSOR_CORN_DIST 225 +#define SENSOR_CORN_ANGLE 90 + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a_rad = position_get_a_rad_double(&mainboard.pos); + double x_corn, y_corn; + int16_t x_corn_int, y_corn_int; + + if (side == I2C_LEFT_SIDE) { + x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + } + else { + x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + } + x_corn_int = x_corn; + y_corn_int = y_corn; + + *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); + if (*corn_idx < 0) + return 0; + return 1; +} + +/* + * - send the correct commands to the spickles + * - return 1 if we need to stop (cobboard is stucked) +*/ +static uint8_t handle_spickles(void) +{ + int8_t corn_idx; + + return 0; + + if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE)) + i2c_cobboard_mode_deploy(I2C_LEFT_SIDE); + else { + if (corn_table[corn_idx] == TYPE_WHITE_CORN) + i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); + else + i2c_cobboard_mode_pack(I2C_LEFT_SIDE); + } +/* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */ +/* time_wait_ms(100); */ + + if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE)) + i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE); + else { + if (corn_table[corn_idx] == TYPE_WHITE_CORN) + i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + else + i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + } + + return 0; +} + +uint8_t line2line(uint8_t dir1, uint8_t num1, + uint8_t dir2, uint8_t num2) +{ + double line1_a_rad, line1_a_deg, line2_a_rad; + double diff_a_deg, diff_a_deg_abs, beta_deg; + double radius; + struct line_2pts l1, l2; + line_t ll1, ll2; + point_t p; + uint8_t err; + + /* convert to 2 points */ + num2line(&l1, dir1, num1); + num2line(&l2, dir2, num2); + + printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), + l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); + printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), + l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); + + /* convert to line eq and find intersection */ + pts2line(&l1.p1, &l1.p2, &ll1); + pts2line(&l2.p1, &l2.p2, &ll2); + intersect_line(&ll1, &ll2, &p); + + line1_a_rad = atan2(l1.p2.y - l1.p1.y, + l1.p2.x - l1.p1.x); + line1_a_deg = DEG(line1_a_rad); + line2_a_rad = atan2(l2.p2.y - l2.p1.y, + l2.p2.x - l2.p1.x); + diff_a_deg = DEG(line2_a_rad - line1_a_rad); + diff_a_deg_abs = fabs(diff_a_deg); + + if (diff_a_deg_abs < 70.) { + radius = 200; + if (diff_a_deg > 0) + beta_deg = 40; + else + beta_deg = -40; + } + else if (diff_a_deg_abs < 100.) { + radius = 100; + if (diff_a_deg > 0) + beta_deg = 40; + else + beta_deg = -40; + } + else { + radius = 120; + if (diff_a_deg > 0) + beta_deg = 60; + else + beta_deg = -60; + } + + trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, + line1_a_deg, 150., diff_a_deg, beta_deg, + radius, xy_norm(l1.p1.x, l1.p1.y, + p.x, p.y)); + err = 0; + while (err == 0) { + err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); + if (err == 0) { + /* cobboard is stucked */ + trajectory_hardstop(&mainboard.traj); + return err; /* XXX do something */ + } + err = test_traj_end(0xFF); + } + return err; +} + +void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) +{ + float n = num; + + switch (dir) { + + case LINE_UP: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(0); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(2100); + break; + case LINE_DOWN: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(2100); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(0); + break; + case LINE_R_UP: + l->p1.x = 150; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 2850; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_L_DOWN: + l->p1.x = 2850; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 150; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + case LINE_L_UP: + l->p1.x = 2850; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 150; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_R_DOWN: + l->p1.x = 150; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 2850; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + default: + break; + } +} + + #if 0 int main(void) { diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h index 8ab2398..def2237 100644 --- a/projects/microb2010/mainboard/strat_corn.h +++ b/projects/microb2010/mainboard/strat_corn.h @@ -29,8 +29,24 @@ #define TYPE_BLACK_CORN 3 #define TYPE_OBSTACLE 4 +#define LINE_UP 0 +#define LINE_DOWN 1 +#define LINE_R_UP 2 +#define LINE_L_DOWN 3 +#define LINE_L_UP 4 +#define LINE_R_DOWN 5 + +struct line_2pts { + point_t p1; + point_t p2; +}; + extern uint8_t corn_table[CORN_NB]; -int8_t ijcoord_to_corn_idx(int8_t i, int8_t j); +int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j); int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y); void init_corn_table(int8_t conf_side, int8_t conf_center); + +uint8_t line2line(uint8_t dir1, uint8_t num1, + uint8_t dir2, uint8_t num2); +void num2line(struct line_2pts *l, uint8_t dir, uint8_t num); -- 2.39.5