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);
}
#include "actuator.h"
#include "cs.h"
#include "i2c_protocol.h"
+#include "state.h"
/* 0 means "programmed"
* ---- with 16 Mhz quartz
printf_P(PSTR("\r\n"));
printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
+
+ state_machine();
cmdline_interact();
return 0;
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
return ball_count;
}
+#if 0
static void state_debug_wait_key_pressed(void)
{
if (!state_debug)
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)
/* harvest balls from area */
static void state_do_harvest(void)
{
- state_debug_wait_key_pressed();
+ //state_debug_wait_key_pressed();
roller_on();
}
&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 */
},
};
/* 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);
/* 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";
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);
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);
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);
/* 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 */
/* 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 */
#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;
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),
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:
save()
elif k == "h":
toggle_obj_disp()
+ elif k == "i":
+ toggle_color()
# EOF
if l == "":
struct genboard gen;
struct mainboard mainboard;
-struct cobboard cobboard;
-struct ballboard ballboard;
+volatile struct cobboard cobboard;
+volatile struct ballboard ballboard;
#ifndef HOST_VERSION
/***********************/
/* 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);
{
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();
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__);
}
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;
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;
}
-/*
+/*
* 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
return "END_UNKNOWN";
}
-void strat_hardstop(void)
+void strat_hardstop(void)
{
trajectory_hardstop(&mainboard.traj);
pid_reset(&mainboard.angle.pid);
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);
DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
}
-/*
+/*
* decrease gain on angle PID, and go forward until we reach the
* border.
*/
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;
}
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);
}
/* start the strat */
void strat_start(void)
-{
+{
int8_t i;
uint8_t err;
break;
}
}
-
+
/* if start sw plugged */
if (!sensor_get(S_START_SWITCH)) {
printf_P(PSTR("Ready, unplug start switch to start\r\n"));
/* 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;
}
uint8_t test_traj_end(uint8_t why)
-{
+{
uint16_t cur_timer;
point_t robot_pt;
}
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;
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;
#include <aversive.h>
#include <aversive/pgmspace.h>
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
+#include <trajectory_manager_core.h>
#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <diagnostic.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#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
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)
{
#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);
git.droids-corp.org / aversive.git / commitdiff