From: zer0 Date: Sat, 24 Apr 2010 21:00:23 +0000 (+0200) Subject: better movements X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=e442b9c066de9b55eef70fdf9993fc3f6b8259e8 better movements --- diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index 879297a..255f26b 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -81,6 +81,7 @@ extern parse_pgm_inst_t cmd_ballboard_setmode3; extern parse_pgm_inst_t cmd_beacon_start; extern parse_pgm_inst_t cmd_servo_balls; extern parse_pgm_inst_t cmd_clitoid; +extern parse_pgm_inst_t cmd_time_monitor; extern parse_pgm_inst_t cmd_test; /* commands_traj.c */ @@ -171,6 +172,7 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_ballboard_setmode3, (parse_pgm_inst_t *)&cmd_servo_balls, (parse_pgm_inst_t *)&cmd_clitoid, + (parse_pgm_inst_t *)&cmd_time_monitor, (parse_pgm_inst_t *)&cmd_test, /* commands_traj.c */ diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c index c749c61..6a3cfc9 100644 --- a/projects/microb2010/mainboard/commands_cs.c +++ b/projects/microb2010/mainboard/commands_cs.c @@ -247,6 +247,7 @@ parse_pgm_inst_t cmd_speed_show = { /* this structure is filled when cmd_derivate_filter is parsed successfully */ struct cmd_derivate_filter_result { struct cmd_cs_result cs; + fixed_string_t show; uint8_t size; }; @@ -273,12 +274,12 @@ static void cmd_derivate_filter_parsed(void *parsed_result, void *show) prog_char str_derivate_filter_arg0[] = "derivate_filter"; parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0); -parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32); +parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT8); prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)"; parse_pgm_inst_t cmd_derivate_filter = { .f = cmd_derivate_filter_parsed, /* function to call */ - .data = (void *)1, /* 2nd arg of func */ + .data = NULL, /* 2nd arg of func */ .help_str = help_derivate_filter, .tokens = { /* token list, NULL terminated */ (prog_void *)&cmd_derivate_filter_arg0, @@ -290,18 +291,13 @@ parse_pgm_inst_t cmd_derivate_filter = { /* show */ -struct cmd_derivate_filter_show_result { - struct cmd_cs_result cs; - fixed_string_t show; -}; - prog_char str_derivate_filter_show_arg[] = "show"; -parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg); +parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, show, str_derivate_filter_show_arg); prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID"; parse_pgm_inst_t cmd_derivate_filter_show = { .f = cmd_derivate_filter_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ + .data = (void *)1, /* 2nd arg of func */ .help_str = help_derivate_filter_show, .tokens = { /* token list, NULL terminated */ (prog_void *)&cmd_derivate_filter_arg0, diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index adec77f..7c58bb9 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1123,6 +1123,47 @@ parse_pgm_inst_t cmd_clitoid = { }, }; +/**********************************************************/ +/* Time_Monitor */ + +/* this structure is filled when cmd_time_monitor is parsed successfully */ +struct cmd_time_monitor_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_time_monitor is parsed successfully */ +static void cmd_time_monitor_parsed(void *parsed_result, void *data) +{ +#ifndef HOST_VERSION + struct cmd_time_monitor_result *res = parsed_result; + uint16_t seconds; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + eeprom_write_word(EEPROM_TIME_ADDRESS, 0); + } + seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); + printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); +#endif +} + +prog_char str_time_monitor_arg0[] = "time_monitor"; +parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0); +prog_char str_time_monitor_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1); + +prog_char help_time_monitor[] = "Show since how long we are running"; +parse_pgm_inst_t cmd_time_monitor = { + .f = cmd_time_monitor_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_time_monitor, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_time_monitor_arg0, + (prog_void *)&cmd_time_monitor_arg1, + NULL, + }, +}; + /**********************************************************/ /* Test */ diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 516f00b..9884bcf 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -224,7 +224,7 @@ void microb_cs_init(void) position_init(&mainboard.pos); position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM); position_set_related_robot_system(&mainboard.pos, &mainboard.rs); - position_set_centrifugal_coef(&mainboard.pos, 0.000016); + // position_set_centrifugal_coef(&mainboard.pos, 0.000016); position_use_ext(&mainboard.pos); /* TRAJECTORY MANAGER */ @@ -240,8 +240,8 @@ void microb_cs_init(void) /* ---- CS angle */ /* PID */ pid_init(&mainboard.angle.pid); - pid_set_gains(&mainboard.angle.pid, 500, 10, 7000); - pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095); + pid_set_gains(&mainboard.angle.pid, 850, 30, 15000); + pid_set_maximums(&mainboard.angle.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.angle.pid, 10); pid_set_derivate_filter(&mainboard.angle.pid, 4); @@ -266,8 +266,8 @@ void microb_cs_init(void) /* ---- CS distance */ /* PID */ pid_init(&mainboard.distance.pid); - pid_set_gains(&mainboard.distance.pid, 500, 10, 7000); - pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095); + pid_set_gains(&mainboard.distance.pid, 850, 30, 15000); + pid_set_maximums(&mainboard.distance.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.distance.pid, 10); pid_set_derivate_filter(&mainboard.distance.pid, 6); diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index 6997139..6ce17cf 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -274,6 +274,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) /* status */ cobboard.mode = ans->mode; cobboard.status = ans->status; + cobboard.cob_count = ans->cob_count; cobboard.left_cobroller_speed = ans->left_cobroller_speed; cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed); cobboard.right_cobroller_speed = ans->right_cobroller_speed; diff --git a/projects/microb2010/mainboard/pid_config.h b/projects/microb2010/mainboard/pid_config.h index fa95f08..251cca8 100755 --- a/projects/microb2010/mainboard/pid_config.h +++ b/projects/microb2010/mainboard/pid_config.h @@ -25,6 +25,6 @@ /** the derivate term can be filtered to remove the noise. This value * is the maxium sample count to keep in memory to do this * filtering. For an instance of pid, this count is defined o*/ -#define PID_DERIVATE_FILTER_MAX_SIZE 4 +#define PID_DERIVATE_FILTER_MAX_SIZE 6 #endif diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index db161b8..2d0d7be 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -57,9 +57,23 @@ static int32_t l_pwm, r_pwm; static int32_t l_enc, r_enc; static int fdr, fdw; +/* + * Debug with GDB: + * + * (gdb) handle SIGUSR1 pass + * Signal Stop Print Pass to program Description + * SIGUSR1 Yes Yes Yes User defined signal 1 + * (gdb) handle SIGUSR2 pass + * Signal Stop Print Pass to program Description + * SIGUSR2 Yes Yes Yes User defined signal 2 + * (gdb) handle SIGUSR1 noprint + * Signal Stop Print Pass to program Description + * SIGUSR1 No No Yes User defined signal 1 + * (gdb) handle SIGUSR2 noprint + */ /* */ -#define FILTER 97 +#define FILTER 98 #define FILTER2 (100-FILTER) #define SHIFT 4 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index e08c4af..52b151e 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -100,6 +100,9 @@ void strat_conf_dump(const char *caller) /* call it just before launching the strat */ void strat_init(void) { + position_set(&mainboard.pos, 298.16, + COLOR_Y(308.78), COLOR_A(70.00)); + /* XXX init rollers, .. */ strat_db_init(); @@ -110,9 +113,9 @@ void strat_init(void) time_reset(); interrupt_traj_reset(); - i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); - i2c_cobboard_harvest(I2C_LEFT_SIDE); - i2c_cobboard_harvest(I2C_RIGHT_SIDE); + //i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + //i2c_cobboard_harvest(I2C_LEFT_SIDE); + //i2c_cobboard_harvest(I2C_RIGHT_SIDE); i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* used in strat_base for END_TIMER */ @@ -163,9 +166,29 @@ static uint8_t strat_beginning(void) { uint8_t err; - strat_set_speed(250, SPEED_ANGLE_FAST); + strat_set_acc(ACC_DIST, ACC_ANGLE); +#ifdef HOST_VERSION + strat_set_speed(600, SPEED_ANGLE_FAST); +#else + /* 250 */ + strat_set_speed(600, SPEED_ANGLE_FAST); +#endif + + + strat_set_speed(600, 60); /* OK */ + // strat_set_speed(250, 28); /* OK */ + trajectory_d_a_rel(&mainboard.traj, 500, COLOR_A(20)); + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_STD); + + strat_set_acc(ACC_DIST, ACC_ANGLE); + +#if 0 l1: + if (get_cob_count() >= 5) + strat_set_speed(600, SPEED_ANGLE_FAST); + err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); if (!TRAJ_SUCCESS(err)) { trajectory_hardstop(&mainboard.traj); @@ -174,6 +197,9 @@ static uint8_t strat_beginning(void) } l2: + if (get_cob_count() >= 5) + strat_set_speed(600, SPEED_ANGLE_FAST); + err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); if (!TRAJ_SUCCESS(err)) { trajectory_hardstop(&mainboard.traj); @@ -182,12 +208,30 @@ static uint8_t strat_beginning(void) } l3: + if (get_cob_count() >= 5) + strat_set_speed(600, SPEED_ANGLE_FAST); + err = line2line(LINE_R_UP, 2, LINE_UP, 5); if (!TRAJ_SUCCESS(err)) { trajectory_hardstop(&mainboard.traj); time_wait_ms(2000); goto l3; } +#else + strat_set_speed(600, SPEED_ANGLE_FAST); + err = line2line(LINE_UP, 0, LINE_R_DOWN, 3); + err = line2line(LINE_R_DOWN, 3, LINE_R_UP, 2); + err = line2line(LINE_R_UP, 2, LINE_R_DOWN, 2); + err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 3); + err = line2line(LINE_R_UP, 3, LINE_UP, 5); + err = line2line(LINE_UP, 5, LINE_L_DOWN, 2); + err = line2line(LINE_L_DOWN, 2, LINE_L_UP, 1); + err = line2line(LINE_L_UP, 1, LINE_L_DOWN, 1); + err = line2line(LINE_L_DOWN, 1, LINE_DOWN, 0); + wait_ms(500); + trajectory_hardstop(&mainboard.traj); + return END_TRAJ; +#endif trajectory_hardstop(&mainboard.traj); diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 42b47dd..ba51434 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -98,8 +98,8 @@ #define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR) /* default acc */ -#define ACC_DIST 10. -#define ACC_ANGLE 10. +#define ACC_DIST 15. +#define ACC_ANGLE 15. /* default speeds */ #define SPEED_DIST_FAST 2500. diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index e661ac1..87bcf9f 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -215,7 +215,7 @@ static struct djpoint *get_neigh(struct djpoint *pt, default: return NULL; } - if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) + if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) return NULL; if (is_reachable(i, j) == 0) diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index 1f3b6e5..6075271 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -103,6 +103,8 @@ const char *get_err(uint8_t err) void strat_hardstop(void) { + DEBUG(E_USER_STRAT, "strat_hardstop"); + trajectory_hardstop(&mainboard.traj); pid_reset(&mainboard.angle.pid); pid_reset(&mainboard.distance.pid); @@ -209,6 +211,11 @@ void strat_set_speed(uint16_t d, uint16_t a) IRQ_UNLOCK(flags); } +void strat_set_acc(double d, double a) +{ + trajectory_set_acc(&mainboard.traj, d, a); +} + void strat_get_speed(uint16_t *d, uint16_t *a) { uint8_t flags; @@ -312,7 +319,6 @@ void strat_start(void) strat_init(); err = strat_main(); - printf("coucou\n"); NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err)); strat_exit(); } @@ -344,9 +350,12 @@ uint8_t strat_obstacle(void) double opp_x, opp_y; #ifdef HOST_VERSION + return 0; if (time_get_s() >= 12 && time_get_s() <= 30) return 1; #endif + return 0; /* XXX disabled */ + if (!sensor_get(S_RCOB_WHITE)) return 0; diff --git a/projects/microb2010/mainboard/strat_base.h b/projects/microb2010/mainboard/strat_base.h index d8b61e5..908a2f6 100644 --- a/projects/microb2010/mainboard/strat_base.h +++ b/projects/microb2010/mainboard/strat_base.h @@ -66,6 +66,7 @@ uint8_t strat_obstacle(void); /* set/get user strat speed */ void strat_set_speed(uint16_t d, uint16_t a); void strat_get_speed(uint16_t *d, uint16_t *a); +void strat_set_acc(double d, double a); /* when user type ctrl-c we can interrupt traj */ void interrupt_traj(void); diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 3694bcd..31afd43 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -161,8 +161,17 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, 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); + if (diff_a_deg < -180) { + diff_a_deg += 360; + } + else if (diff_a_deg > 180) { + diff_a_deg -= 360; + } diff_a_deg_abs = fabs(diff_a_deg); +/* printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */ +/* printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */ + if (diff_a_deg_abs < 70.) { radius = 200; if (diff_a_deg > 0) @@ -185,10 +194,12 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, beta_deg = -60; } + /* XXX check return value !! */ 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)); + /* disabled */ if (0) { err = 0; while (err == 0) {