/* 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;
gen.logs[NB_LOGS] = 0;
gen.log_level = old_level;
-#endif
}
prog_char str_start_arg0[] = "start";
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)
{
}
#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);
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";
git.droids-corp.org / aversive.git / blobdiff