X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;fp=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=de7351808792e64a1927fe15dab4dc8b202a17fc;hp=7c42411a5a613bd97bb6c740b7a811f0e29caf42;hb=78150017ab8c5615af414df706a0525fe7c262ae;hpb=b022f257a5ee568737e1a684a83d0154397fffcb 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";