From 6914527de2ecfef9d790740c71739e7418246b96 Mon Sep 17 00:00:00 2001 From: zer0 Date: Tue, 13 Apr 2010 20:16:32 +0200 Subject: [PATCH 1/1] save --- .../blocking_detection_manager.c | 69 +-- .../trajectory_manager/trajectory_manager.h | 2 +- .../trajectory_manager_core.c | 96 ++-- .../trajectory_manager_core.h | 30 ++ projects/microb2010/ballboard/actuator.c | 6 +- projects/microb2010/ballboard/cmdline.h | 2 +- .../microb2010/ballboard/commands_ballboard.c | 58 +-- projects/microb2010/ballboard/commands_gen.c | 10 +- projects/microb2010/ballboard/cs.c | 37 +- projects/microb2010/ballboard/main.c | 29 +- projects/microb2010/ballboard/main.h | 5 +- projects/microb2010/ballboard/state.c | 2 + projects/microb2010/cobboard/actuator.c | 8 + projects/microb2010/cobboard/actuator.h | 1 + .../microb2010/cobboard/commands_cobboard.c | 100 ++-- projects/microb2010/cobboard/cs.c | 18 +- projects/microb2010/cobboard/i2c_protocol.c | 2 +- projects/microb2010/cobboard/main.c | 26 +- projects/microb2010/cobboard/main.h | 1 + projects/microb2010/cobboard/state.c | 112 +++-- projects/microb2010/cobboard/state.h | 1 + projects/microb2010/common/i2c_commands.h | 9 +- projects/microb2010/mainboard/commands.c | 12 + .../microb2010/mainboard/commands_mainboard.c | 381 ++++----------- projects/microb2010/mainboard/commands_traj.c | 441 ++++++++++++++---- projects/microb2010/mainboard/cs.c | 11 +- projects/microb2010/mainboard/display.py | 90 +++- projects/microb2010/mainboard/i2c_protocol.c | 7 +- projects/microb2010/mainboard/main.c | 28 +- projects/microb2010/mainboard/main.h | 1 + projects/microb2010/mainboard/robotsim.c | 78 +++- projects/microb2010/mainboard/robotsim.h | 5 +- projects/microb2010/mainboard/strat.c | 26 +- projects/microb2010/mainboard/strat.h | 15 +- 34 files changed, 1064 insertions(+), 655 deletions(-) diff --git a/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c b/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c index cdb2b58..8b5f547 100644 --- a/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c +++ b/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation (2007) - * + * * 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 @@ -38,8 +38,8 @@ void bd_init(struct blocking_detection * bd) } /* thresholds */ -void bd_set_current_thresholds(struct blocking_detection * bd, - int32_t k1, int32_t k2, +void bd_set_current_thresholds(struct blocking_detection * bd, + int32_t k1, int32_t k2, uint32_t i_thres, uint16_t cpt_thres) { uint8_t flags; @@ -53,7 +53,7 @@ void bd_set_current_thresholds(struct blocking_detection * bd, } /* speed threshold */ -void bd_set_speed_threshold(struct blocking_detection * bd, +void bd_set_speed_threshold(struct blocking_detection * bd, uint16_t speed) { uint8_t flags; @@ -71,45 +71,48 @@ void bd_reset(struct blocking_detection * bd) IRQ_UNLOCK(flags); } - - /** function to be called periodically */ -void bd_manage_from_speed_cmd(struct blocking_detection * bd, - int32_t speed, int32_t cmd) +void bd_manage_from_speed_cmd(struct blocking_detection * bd, + int32_t speed, int32_t cmd) { - int32_t i=0; + int32_t i = 0; + + /* disabled */ + if (bd->cpt_thres == 0) + return; - /* if current-based blocking_detection enabled */ - if ( bd->cpt_thres ) { - i = bd->k1 * cmd - bd->k2 * speed; - if ((uint32_t)ABS(i) > bd->i_thres && - (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres)) { - if (bd->cpt == bd->cpt_thres - 1) - WARNING(E_BLOCKING_DETECTION_MANAGER, - "BLOCKING cmd=%ld, speed=%ld i=%ld", - cmd, speed, i); - if (bd->cpt < bd->cpt_thres) - bd->cpt++; - } - else { - bd->cpt=0; - } + i = bd->k1 * cmd - bd->k2 * speed; + + /* if i is above threshold, speed is below threshold, and cmd + * has the same sign than i */ + if ((uint32_t)ABS(i) > bd->i_thres && + (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres) && + (i * cmd > 0)) { + if (bd->cpt == bd->cpt_thres - 1) + WARNING(E_BLOCKING_DETECTION_MANAGER, + "BLOCKING cmd=%ld, speed=%ld i=%ld", + cmd, speed, i); + if (bd->cpt < bd->cpt_thres) + bd->cpt++; + } + else { + bd->cpt=0; + } #if BD_DEBUG - if (bd->debug_cpt++ == BD_DEBUG) { - DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld", - cmd, speed, i); - bd->debug_cpt = 0; - } + if (bd->debug_cpt++ == BD_DEBUG) { + DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld", + cmd, speed, i); + bd->debug_cpt = 0; } #endif } /** function to be called periodically */ -void bd_manage_from_pos_cmd(struct blocking_detection * bd, - int32_t pos, int32_t cmd) +void bd_manage_from_pos_cmd(struct blocking_detection * bd, + int32_t pos, int32_t cmd) { int32_t speed = (pos - bd->prev_pos); - bd_manage_from_speed_cmd(bd, speed, cmd); + bd_manage_from_speed_cmd(bd, speed, cmd); bd->prev_pos = pos; } diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index 594b7b0..3e22502 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -130,7 +130,7 @@ void trajectory_set_robot_params(struct trajectory *traj, void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed); /** set speed consign */ -void trajectory_set_speed(struct trajectory *traj, double d_acc, double a_acc); +void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc); /** * set windows for trajectory. diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 5246285..5197300 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -643,13 +643,11 @@ static void trajectory_manager_line_event(struct trajectory *traj) } /* position consign is infinite */ - d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm)); + d_consign = pos_mm2imp(traj, v2pol_target.r); d_consign += rs_get_distance(traj->robot); - /* angle consign */ - a_consign = (int32_t)(v2pol_target.theta * - (traj->position->phys.distance_imp_per_mm) * - (traj->position->phys.track_mm) / 2.2); + /* angle consign (1.1 to avoid oscillations) */ + a_consign = pos_rd2imp(traj, v2pol_target.theta) / 1.1; a_consign += rs_get_angle(traj->robot); EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f " @@ -849,7 +847,7 @@ void trajectory_line_abs(struct trajectory *traj, * - Va_out: the angular speed to configure in quadramp * - remain_d_mm_out: remaining distance before start to turn */ -static uint8_t calc_clitoid(struct trajectory *traj, +static int8_t calc_clitoid(struct trajectory *traj, double x, double y, double a_rad, double alpha_deg, double beta_deg, double R_mm, double Vd, double Amax, double d_inter_mm, @@ -857,12 +855,14 @@ static uint8_t calc_clitoid(struct trajectory *traj, { double Vd_mm_s; double Va, Va_rd_s; - double t, d_mm, alpha_rad, beta_rad; + double t, tau, d_mm, alpha_rad, beta_rad; double remain_d_mm; double Aa, Aa_rd_s2; line_t line1, line2; - point_t robot, intersect, pt2, center, proj; + line_t line1_int, line2_int; + point_t robot, intersect, pt2, center, proj, M; vect_t v; + double xm, ym, L, A; /* param check */ if (fabs(alpha_deg) <= fabs(beta_deg)) { @@ -896,11 +896,6 @@ static uint8_t calc_clitoid(struct trajectory *traj, return -1; } - /* the robot position */ -/* x = position_get_x_double(&mainboard.pos); */ -/* y = position_get_y_double(&mainboard.pos); */ -/* a_rad = position_get_a_rad_double(&mainboard.pos); */ - /* define line1 and line2 */ robot.x = x; robot.y = y; @@ -913,46 +908,74 @@ static uint8_t calc_clitoid(struct trajectory *traj, DEBUG(E_TRAJECTORY, "intersect=(%2.2f, %2.2f)", intersect.x, intersect.y); - /* the center of the circle is at (d_mm, d_mm) when we have to - * start the clothoid */ - d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) * - sqrt(M_PI) / 2.; + /* L and A are the parameters of the clothoid, xm and ym are + * the relative coords (starting from the beginning of + * clothoid) of the crossing point between the clothoid and + * the circle. */ + L = Vd_mm_s * t; + A = R_mm * sqrt(fabs(alpha_rad - beta_rad)); + xm = + L + - (pow(L, 5) / (40. * pow(A, 4))) + + (pow(L, 9) / (3456. * pow(A, 8))) + - (pow(L, 13) / (599040. * pow(A, 12))); + ym = + (pow(L, 3) / (6. * pow(A, 2))) + - (pow(L, 7) / (336. * pow(A, 6))) + + (pow(L, 11) / (42240. * pow(A, 10))) + - (pow(L, 15) / (9676800. * pow(A, 14))); + DEBUG(E_TRAJECTORY, "relative xm,ym = (%2.2f, %2.2f)", + xm, ym); + + /* the center of the circle is at d_mm when we have to start + * the clothoid */ + tau = (alpha_rad - beta_rad) / 2.; + d_mm = ym + (R_mm * cos(tau)); DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm); /* translate line1 */ + memcpy(&line1_int, &line1, sizeof(line1_int)); + memcpy(&line2_int, &line2, sizeof(line2_int)); v.x = intersect.x - robot.x; v.y = intersect.y - robot.y; - if (a_rad > 0) + if (alpha_rad > 0) vect_rot_trigo(&v); else vect_rot_retro(&v); vect_resize(&v, d_mm); - line_translate(&line1, &v); + line_translate(&line1_int, &v); + DEBUG(E_TRAJECTORY, "translate line1 by %2.2f,%2.2f", v.x, v.y); - /* translate line2 */ + /* translate line2_int */ v.x = intersect.x - pt2.x; v.y = intersect.y - pt2.y; - if (a_rad > 0) + if (alpha_rad < 0) vect_rot_trigo(&v); else vect_rot_retro(&v); vect_resize(&v, d_mm); - line_translate(&line2, &v); + line_translate(&line2_int, &v); + DEBUG(E_TRAJECTORY, "translate line2 by %2.2f,%2.2f", v.x, v.y); /* find the center of the circle, at the intersection of the * new translated lines */ - if (intersect_line(&line1, &line2, ¢er) != 1) { + if (intersect_line(&line1_int, &line2_int, ¢er) != 1) { DEBUG(E_TRAJECTORY, "cannot find circle center"); return -1; } DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y); - /* project center of circle on line1 */ - proj_pt_line(¢er, &line1, &proj); - DEBUG(E_TRAJECTORY, "proj=(%2.2f,%2.2f)", proj.x, proj.y); + /* M is the same point than xm, ym but in absolute coords */ + M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm; + M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm; + DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y); + + /* project M on line 1 */ + proj_pt_line(&M, &line1, &proj); + DEBUG(E_TRAJECTORY, "proj M = (%2.2f, %2.2f)", proj.x, proj.y); /* process remaining distance before start turning */ - remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm); + remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + xm); DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm); if (remain_d_mm < 0) { DEBUG(E_TRAJECTORY, "too late, cannot turn"); @@ -966,6 +989,7 @@ static uint8_t calc_clitoid(struct trajectory *traj, return 0; } +/* after the line, start the clothoid */ static void start_clitoid(struct trajectory *traj) { double Aa = traj->target.line.Aa; @@ -975,12 +999,14 @@ static void start_clitoid(struct trajectory *traj) double d; delete_event(traj); + DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f", + __FUNCTION__, Va, Aa); traj->state = RUNNING_CLITOID_CURVE; - set_quadramp_acc(traj, Aa, Aa); - set_quadramp_speed(traj, Va, Va); - d = R_mm * a_rad; - d *= 2.; /* margin to avoid deceleration */ + d = fabs(R_mm * a_rad); + d *= 3.; /* margin to avoid deceleration */ trajectory_d_a_rel(traj, d, DEG(a_rad)); + set_quadramp_acc(traj, traj->d_acc, Aa); + set_quadramp_speed(traj, traj->d_speed, Va); } @@ -1010,11 +1036,12 @@ static void start_clitoid(struct trajectory *traj) int8_t trajectory_clitoid(struct trajectory *traj, double x, double y, double a, double advance, double alpha_deg, double beta_deg, double R_mm, - double Vd, double d_inter_mm) + double d_inter_mm) { - double remain = 0, Aa = 0, Va = 0; + double remain = 0, Aa = 0, Va = 0, Vd; double turnx, turny; + Vd = traj->d_speed; if (calc_clitoid(traj, x, y, a, alpha_deg, beta_deg, R_mm, Vd, traj->a_acc, d_inter_mm, &Aa, &Va, &remain) < 0) @@ -1029,6 +1056,9 @@ int8_t trajectory_clitoid(struct trajectory *traj, traj->target.line.R = R_mm; traj->target.line.turn_pt.x = turnx; traj->target.line.turn_pt.y = turny; + DEBUG(E_TRAJECTORY, "%s() turn_pt=%2.2f,%2.2f", + __FUNCTION__, turnx, turny); + __trajectory_line_abs(traj, x, y, turnx, turny, advance); traj->state = RUNNING_CLITOID_LINE; diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.h b/modules/devices/robot/trajectory_manager/trajectory_manager_core.h index a724f69..b5a1395 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.h @@ -111,3 +111,33 @@ void trajectory_manager_event(void * param); /*********** *CIRCLE */ /* XXX TODO */ + +/*********** CLITOID */ + +/** + * do a superb curve joining line1 to line2 which is composed of: + * - a clothoid starting from line1 + * - a circle + * - another clothoid up to line2 + * this curve is called a clitoid (hehe) + * + * the function assumes that the initial linear speed is Vd and + * angular speed is 0. + * + * - x,y,a: starting position + * - advance: parameter for line following + * - alpha: total angle + * - beta: circular part of angle (lower than alpha) + * - R: the radius of the circle (must be != 0) + * - Vd: linear speed to use (in imp per cs period) + * - Amax: maximum angular acceleration + * - d_inter: distance in mm until the intersection of the + * 2 lines + * + * return 0 if trajectory can be loaded, then it is processed in + * background. + */ +int8_t trajectory_clitoid(struct trajectory *traj, + double x, double y, double a, double advance, + double alpha_deg, double beta_deg, double R_mm, + double d_inter_mm); diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c index 2509d7c..7365427 100644 --- a/projects/microb2010/ballboard/actuator.c +++ b/projects/microb2010/ballboard/actuator.c @@ -42,11 +42,11 @@ #include "main.h" -#define ROLLER_ON 800 +#define ROLLER_ON -1200 #define ROLLER_OFF 0 -#define ROLLER_REVERSE -800 +#define ROLLER_REVERSE 1200 -#define FORKROT_DEPLOYED 1000 +#define FORKROT_DEPLOYED -50000 #define FORKROT_PACKED 0 #define FORKTRANS_LEFT 0 diff --git a/projects/microb2010/ballboard/cmdline.h b/projects/microb2010/ballboard/cmdline.h index 57dfa49..04ce1f3 100644 --- a/projects/microb2010/ballboard/cmdline.h +++ b/projects/microb2010/ballboard/cmdline.h @@ -35,6 +35,6 @@ static inline uint8_t cmdline_keypressed(void) { return (uart_recv_nowait(CMDLINE_UART) != -1); } -static inline uint8_t cmdline_getchar(void) { +static inline int16_t cmdline_getchar(void) { return uart_recv_nowait(CMDLINE_UART); } diff --git a/projects/microb2010/ballboard/commands_ballboard.c b/projects/microb2010/ballboard/commands_ballboard.c index fd45914..ba570a4 100644 --- a/projects/microb2010/ballboard/commands_ballboard.c +++ b/projects/microb2010/ballboard/commands_ballboard.c @@ -1,6 +1,6 @@ /* * Copyright Droids Corporation (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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_ballboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -63,22 +63,24 @@ static void cmd_event_parsed(void *parsed_result, void *data) u08 bit=0; struct cmd_event_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("all"))) { - bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; + bit = 0xFF; if (!strcmp_P(res->arg2, PSTR("on"))) ballboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) ballboard.flags &= bit; else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), + printf_P(PSTR("encoders is %s\r\n"), (DO_ENCODERS & ballboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), + printf_P(PSTR("cs is %s\r\n"), (DO_CS & ballboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), + printf_P(PSTR("bd is %s\r\n"), (DO_BD & ballboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), + printf_P(PSTR("power is %s\r\n"), (DO_POWER & ballboard.flags) ? "on":"off"); + printf_P(PSTR("errblock is %s\r\n"), + (DO_ERRBLOCKING & ballboard.flags) ? "on":"off"); } return; } @@ -92,6 +94,8 @@ static void cmd_event_parsed(void *parsed_result, void *data) bit = DO_BD; else if (!strcmp_P(res->arg1, PSTR("power"))) bit = DO_POWER; + else if (!strcmp_P(res->arg1, PSTR("errblock"))) + bit = DO_ERRBLOCKING; if (!strcmp_P(res->arg2, PSTR("on"))) @@ -104,13 +108,13 @@ static void cmd_event_parsed(void *parsed_result, void *data) } ballboard.flags &= (~bit); } - printf_P(PSTR("%s is %s\r\n"), res->arg1, + printf_P(PSTR("%s is %s\r\n"), res->arg1, (bit & ballboard.flags) ? "on":"off"); } prog_char str_event_arg0[] = "event"; parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); -prog_char str_event_arg1[] = "all#encoders#cs#bd#power"; +prog_char str_event_arg1[] = "all#encoders#cs#bd#power#errblock"; parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); prog_char str_event_arg2[] = "on#off#show"; parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); @@ -121,9 +125,9 @@ parse_pgm_inst_t cmd_event = { .data = NULL, /* 2nd arg of func */ .help_str = help_event, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_event_arg0, - (prog_void *)&cmd_event_arg1, - (prog_void *)&cmd_event_arg2, + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, NULL, }, }; @@ -161,8 +165,8 @@ parse_pgm_inst_t cmd_color = { .data = NULL, /* 2nd arg of func */ .help_str = help_color, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_color_arg0, - (prog_void *)&cmd_color_color, + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, NULL, }, }; @@ -275,7 +279,7 @@ struct cmd_state3_result { }; /* function called when cmd_state3 is parsed successfully */ -static void cmd_state3_parsed(void *parsed_result, +static void cmd_state3_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state3_result *res = parsed_result; @@ -300,9 +304,9 @@ parse_pgm_inst_t cmd_state3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state3_arg0, - (prog_void *)&cmd_state3_arg1, - (prog_void *)&cmd_state3_arg2, + (prog_void *)&cmd_state3_arg0, + (prog_void *)&cmd_state3_arg1, + (prog_void *)&cmd_state3_arg2, NULL, }, }; @@ -331,7 +335,7 @@ parse_pgm_inst_t cmd_state_machine = { .data = NULL, /* 2nd arg of func */ .help_str = help_state_machine, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_machine_arg0, + (prog_void *)&cmd_state_machine_arg0, NULL, }, }; @@ -363,8 +367,8 @@ parse_pgm_inst_t cmd_state_debug = { .data = NULL, /* 2nd arg of func */ .help_str = help_state_debug, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_debug_arg0, - (prog_void *)&cmd_state_debug_on, + (prog_void *)&cmd_state_debug_arg0, + (prog_void *)&cmd_state_debug_on, NULL, }, }; @@ -406,8 +410,8 @@ parse_pgm_inst_t cmd_fork = { .data = NULL, /* 2nd arg of func */ .help_str = help_fork, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_fork_arg0, - (prog_void *)&cmd_fork_arg1, + (prog_void *)&cmd_fork_arg0, + (prog_void *)&cmd_fork_arg1, NULL, }, }; @@ -445,8 +449,8 @@ parse_pgm_inst_t cmd_roller = { .data = NULL, /* 2nd arg of func */ .help_str = help_roller, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_roller_arg0, - (prog_void *)&cmd_roller_arg1, + (prog_void *)&cmd_roller_arg0, + (prog_void *)&cmd_roller_arg1, NULL, }, }; @@ -474,7 +478,7 @@ parse_pgm_inst_t cmd_test = { .data = NULL, /* 2nd arg of func */ .help_str = help_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_test_arg0, + (prog_void *)&cmd_test_arg0, NULL, }, }; diff --git a/projects/microb2010/ballboard/commands_gen.c b/projects/microb2010/ballboard/commands_gen.c index b2a1e5d..66dccc1 100644 --- a/projects/microb2010/ballboard/commands_gen.c +++ b/projects/microb2010/ballboard/commands_gen.c @@ -357,9 +357,7 @@ static const prog_char i2c_log[] = "i2c"; static const prog_char i2cproto_log[] = "i2cproto"; static const prog_char sensor_log[] = "sensor"; static const prog_char block_log[] = "bd"; -static const prog_char beacon_log[] = "beacon"; -static const prog_char scanner_log[] = "scanner"; -static const prog_char imgprocess_log[] = "imgprocess"; +static const prog_char state_log[] = "state"; struct log_name_and_num { const prog_char * name; @@ -372,9 +370,7 @@ static const struct log_name_and_num log_name_and_num[] = { { i2cproto_log, E_USER_I2C_PROTO }, { sensor_log, E_USER_SENSOR }, { block_log, E_BLOCKING_DETECTION_MANAGER }, - { beacon_log, E_USER_BEACON }, - { scanner_log, E_USER_SCANNER }, - { imgprocess_log, E_USER_IMGPROCESS }, + { state_log, E_USER_ST_MACH }, }; static uint8_t @@ -523,7 +519,7 @@ static void cmd_log_type_parsed(void * parsed_result, void * data) prog_char str_log_arg1_type[] = "type"; parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type); /* keep it sync with log_name_and_num above */ -prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#beacon#scanner#imgprocess"; +prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#state"; parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type); prog_char str_log_arg3[] = "on#off"; parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3); diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c index bab700d..cf122e6 100644 --- a/projects/microb2010/ballboard/cs.c +++ b/projects/microb2010/ballboard/cs.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation * Olivier Matz - * + * * 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 @@ -72,10 +72,20 @@ static void do_cs(void *dummy) if (ballboard.forkrot.on) cs_manage(&ballboard.forkrot.cs); } - if (ballboard.flags & DO_BD) { + if ((ballboard.flags & DO_BD) && (ballboard.flags & DO_POWER)) { bd_manage_from_cs(&ballboard.roller.bd, &ballboard.roller.cs); bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs); bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs); + + /* urgent case: stop power on blocking */ + if (ballboard.flags & DO_ERRBLOCKING) { + if (bd_get(&ballboard.roller.bd) || + bd_get(&ballboard.forktrans.bd) || + bd_get(&ballboard.forkrot.bd)) { + printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); + ballboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); + } + } } if (ballboard.flags & DO_POWER) BRAKE_OFF(); @@ -86,7 +96,7 @@ static void do_cs(void *dummy) void dump_cs(const char *name, struct cs *cs) { printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld\r\n"), + "in=% .5ld out=% .5ld\r\n"), name, cs_get_consign(cs), cs_get_filtered_consign(cs), cs_get_error(cs), cs_get_filtered_feedback(cs), cs_get_out(cs)); @@ -119,10 +129,15 @@ void microb_cs_init(void) cs_set_process_out(&ballboard.roller.cs, encoders_spi_update_roller_speed, ROLLER_ENCODER); cs_set_consign(&ballboard.roller.cs, 0); + /* Blocking detection */ + bd_init(&ballboard.roller.bd); + bd_set_speed_threshold(&ballboard.roller.bd, 150); + bd_set_current_thresholds(&ballboard.roller.bd, 500, 8000, 1000000, 200); + /* ---- CS forktrans */ /* PID */ pid_init(&ballboard.forktrans.pid); - pid_set_gains(&ballboard.forktrans.pid, 200, 5, 250); + pid_set_gains(&ballboard.forktrans.pid, 30, 5, 0); pid_set_maximums(&ballboard.forktrans.pid, 0, 10000, 2047); pid_set_out_shift(&ballboard.forktrans.pid, 6); pid_set_derivate_filter(&ballboard.forktrans.pid, 6); @@ -143,19 +158,19 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&ballboard.forktrans.bd); bd_set_speed_threshold(&ballboard.forktrans.bd, 150); - bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 200); /* ---- CS forkrot */ /* PID */ pid_init(&ballboard.forkrot.pid); - pid_set_gains(&ballboard.forkrot.pid, 200, 5, 250); + pid_set_gains(&ballboard.forkrot.pid, 30, 5, 0); pid_set_maximums(&ballboard.forkrot.pid, 0, 10000, 2047); pid_set_out_shift(&ballboard.forkrot.pid, 6); pid_set_derivate_filter(&ballboard.forkrot.pid, 6); /* QUADRAMP */ quadramp_init(&ballboard.forkrot.qr); - quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 200); /* set speed */ + quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 800, 800); /* set speed */ quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */ /* CS */ @@ -169,15 +184,15 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&ballboard.forkrot.bd); bd_set_speed_threshold(&ballboard.forkrot.bd, 150); - bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 200); /* set them on !! */ - ballboard.roller.on = 0; + ballboard.roller.on = 1; ballboard.forktrans.on = 1; ballboard.forkrot.on = 1; scheduler_add_periodical_event_priority(do_cs, NULL, - 5000L / SCHEDULER_UNIT, + 5000L / SCHEDULER_UNIT, CS_PRIO); } diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c index 082b658..ed8716b 100755 --- a/projects/microb2010/ballboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -112,16 +112,21 @@ void bootloader(void) void do_led_blink(__attribute__((unused)) void *dummy) { -#if 1 /* simple blink */ - static uint8_t a=0; + static uint8_t a = 0; - if(a) - LED1_ON(); - else - LED1_OFF(); - - a = !a; -#endif + if (ballboard.flags & DO_ERRBLOCKING) { + if (a & 1) + LED1_ON(); + else + LED1_OFF(); + } + else { + if (a & 4) + LED1_ON(); + else + LED1_OFF(); + } + a++; } static void main_timer_interrupt(void) @@ -149,7 +154,8 @@ int main(void) LED1_OFF(); memset(&gen, 0, sizeof(gen)); memset(&ballboard, 0, sizeof(ballboard)); - ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER; // DO_BD + ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER | + DO_ERRBLOCKING | DO_BD; /* UART */ uart_init(); @@ -239,6 +245,9 @@ int main(void) /* ax12 */ ax12_user_init(); + gen.logs[0] = E_USER_ST_MACH; + gen.log_level = 5; + sei(); printf_P(PSTR("\r\n")); diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h index 3ebb1e7..cde4608 100755 --- a/projects/microb2010/ballboard/main.h +++ b/projects/microb2010/ballboard/main.h @@ -60,9 +60,7 @@ /** ERROR NUMS */ #define E_USER_I2C_PROTO 195 #define E_USER_SENSOR 196 -#define E_USER_BEACON 197 -#define E_USER_SCANNER 198 -#define E_USER_IMGPROCESS 199 +#define E_USER_ST_MACH 197 #define LED_PRIO 170 #define TIME_PRIO 160 @@ -115,6 +113,7 @@ struct ballboard { #define DO_CS 2 #define DO_BD 4 #define DO_POWER 8 +#define DO_ERRBLOCKING 16 uint8_t flags; /* misc flags */ /* control systems */ diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c index 9568be8..0492e18 100644 --- a/projects/microb2010/ballboard/state.c +++ b/projects/microb2010/ballboard/state.c @@ -94,6 +94,7 @@ static void state_debug_wait_key_pressed(void) int8_t state_set_mode(uint8_t mode) { state_mode = mode; + STMCH_DEBUG("%s(): mode=%x ", __FUNCTION__, mode); /* STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */ /* "r_deploy=%d r_harvest=%d eject=%d", */ @@ -135,6 +136,7 @@ static void state_do_harvest(void) static void state_do_eject(void) { roller_reverse(); + time_wait_ms(2000); } /* main state machine */ diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index 6811854..1b6e91f 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -103,6 +103,14 @@ void cobroller_off(uint8_t side) cobboard.right_cobroller_speed = 0; } +void cobroller_reverse(uint8_t side) +{ + if (side == I2C_LEFT_SIDE) + cobboard.left_cobroller_speed = -COBROLLER_SPEED; + else + cobboard.right_cobroller_speed = COBROLLER_SPEED; +} + void actuator_init(void) { diff --git a/projects/microb2010/cobboard/actuator.h b/projects/microb2010/cobboard/actuator.h index 5b59a73..4896447 100644 --- a/projects/microb2010/cobboard/actuator.h +++ b/projects/microb2010/cobboard/actuator.h @@ -33,6 +33,7 @@ void servo_door_block(void); void cobroller_on(uint8_t side); void cobroller_off(uint8_t side); +void cobroller_reverse(uint8_t side); #endif diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index 4ea3b80..bea27e9 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -1,6 +1,6 @@ /* * Copyright Droids Corporation (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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_cobboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -68,20 +68,22 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * struct cmd_event_result * res = parsed_result; if (!strcmp_P(res->arg1, PSTR("all"))) { - bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; + bit = 0xFF; if (!strcmp_P(res->arg2, PSTR("on"))) cobboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) cobboard.flags &= bit; else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), + printf_P(PSTR("encoders is %s\r\n"), (DO_ENCODERS & cobboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), + printf_P(PSTR("cs is %s\r\n"), (DO_CS & cobboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), + printf_P(PSTR("bd is %s\r\n"), (DO_BD & cobboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), + printf_P(PSTR("power is %s\r\n"), (DO_POWER & cobboard.flags) ? "on":"off"); + printf_P(PSTR("errblock is %s\r\n"), + (DO_ERRBLOCKING & cobboard.flags) ? "on":"off"); } return; } @@ -95,6 +97,8 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * bit = DO_BD; else if (!strcmp_P(res->arg1, PSTR("power"))) bit = DO_POWER; + else if (!strcmp_P(res->arg1, PSTR("errblock"))) + bit = DO_ERRBLOCKING; if (!strcmp_P(res->arg2, PSTR("on"))) @@ -107,13 +111,13 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * } cobboard.flags &= (~bit); } - printf_P(PSTR("%s is %s\r\n"), res->arg1, + printf_P(PSTR("%s is %s\r\n"), res->arg1, (bit & cobboard.flags) ? "on":"off"); } prog_char str_event_arg0[] = "event"; parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); -prog_char str_event_arg1[] = "all#encoders#cs#bd#power"; +prog_char str_event_arg1[] = "all#encoders#cs#bd#power#errblock"; parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); prog_char str_event_arg2[] = "on#off#show"; parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); @@ -124,9 +128,9 @@ parse_pgm_inst_t cmd_event = { .data = NULL, /* 2nd arg of func */ .help_str = help_event, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_event_arg0, - (prog_void *)&cmd_event_arg1, - (prog_void *)&cmd_event_arg2, + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, NULL, }, }; @@ -164,8 +168,8 @@ parse_pgm_inst_t cmd_color = { .data = NULL, /* 2nd arg of func */ .help_str = help_color, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_color_arg0, - (prog_void *)&cmd_color_color, + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, NULL, }, }; @@ -204,8 +208,8 @@ parse_pgm_inst_t cmd_state1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state1_arg0, - (prog_void *)&cmd_state1_arg1, + (prog_void *)&cmd_state1_arg0, + (prog_void *)&cmd_state1_arg1, NULL, }, }; @@ -272,9 +276,9 @@ parse_pgm_inst_t cmd_state2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state2_arg0, - (prog_void *)&cmd_state2_arg1, - (prog_void *)&cmd_state2_arg2, + (prog_void *)&cmd_state2_arg0, + (prog_void *)&cmd_state2_arg1, + (prog_void *)&cmd_state2_arg2, NULL, }, }; @@ -290,7 +294,7 @@ struct cmd_state3_result { }; /* function called when cmd_state3 is parsed successfully */ -static void cmd_state3_parsed(void *parsed_result, +static void cmd_state3_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state3_result *res = parsed_result; @@ -315,9 +319,9 @@ parse_pgm_inst_t cmd_state3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state3_arg0, - (prog_void *)&cmd_state3_arg1, - (prog_void *)&cmd_state3_arg2, + (prog_void *)&cmd_state3_arg0, + (prog_void *)&cmd_state3_arg1, + (prog_void *)&cmd_state3_arg2, NULL, }, }; @@ -346,7 +350,7 @@ parse_pgm_inst_t cmd_state_machine = { .data = NULL, /* 2nd arg of func */ .help_str = help_state_machine, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_machine_arg0, + (prog_void *)&cmd_state_machine_arg0, NULL, }, }; @@ -378,8 +382,8 @@ parse_pgm_inst_t cmd_state_debug = { .data = NULL, /* 2nd arg of func */ .help_str = help_state_debug, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_debug_arg0, - (prog_void *)&cmd_state_debug_on, + (prog_void *)&cmd_state_debug_arg0, + (prog_void *)&cmd_state_debug_on, NULL, }, }; @@ -417,8 +421,8 @@ parse_pgm_inst_t cmd_servo_door = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_door, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_door_arg0, - (prog_void *)&cmd_servo_door_arg1, + (prog_void *)&cmd_servo_door_arg0, + (prog_void *)&cmd_servo_door_arg1, NULL, }, }; @@ -464,9 +468,9 @@ parse_pgm_inst_t cmd_cobroller = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobroller, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobroller_arg0, - (prog_void *)&cmd_cobroller_arg1, - (prog_void *)&cmd_cobroller_arg2, + (prog_void *)&cmd_cobroller_arg0, + (prog_void *)&cmd_cobroller_arg1, + (prog_void *)&cmd_cobroller_arg2, NULL, }, }; @@ -504,8 +508,8 @@ parse_pgm_inst_t cmd_shovel = { .data = NULL, /* 2nd arg of func */ .help_str = help_shovel, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_shovel_arg0, - (prog_void *)&cmd_shovel_arg1, + (prog_void *)&cmd_shovel_arg0, + (prog_void *)&cmd_shovel_arg1, NULL, }, }; @@ -541,8 +545,8 @@ parse_pgm_inst_t cmd_servo_carry = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_carry, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_carry_arg0, - (prog_void *)&cmd_servo_carry_arg1, + (prog_void *)&cmd_servo_carry_arg0, + (prog_void *)&cmd_servo_carry_arg1, NULL, }, }; @@ -597,9 +601,9 @@ parse_pgm_inst_t cmd_spickle = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_arg0, - (prog_void *)&cmd_spickle_arg1, - (prog_void *)&cmd_spickle_arg2, + (prog_void *)&cmd_spickle_arg0, + (prog_void *)&cmd_spickle_arg1, + (prog_void *)&cmd_spickle_arg2, NULL, }, }; @@ -680,8 +684,8 @@ parse_pgm_inst_t cmd_spickle_params_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle_params_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_params_arg0, - (prog_void *)&cmd_spickle_params_arg1_show, + (prog_void *)&cmd_spickle_params_arg0, + (prog_void *)&cmd_spickle_params_arg1_show, NULL, }, }; @@ -702,7 +706,7 @@ static void cmd_spickle_params2_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_spickle_params2_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("coef"))) { spickle_set_coefs(res->arg2, res->arg3); } @@ -728,10 +732,10 @@ parse_pgm_inst_t cmd_spickle_params2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle_params2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_params2_arg0, - (prog_void *)&cmd_spickle_params2_arg1, - (prog_void *)&cmd_spickle_params2_arg2, - (prog_void *)&cmd_spickle_params2_arg3, + (prog_void *)&cmd_spickle_params2_arg0, + (prog_void *)&cmd_spickle_params2_arg1, + (prog_void *)&cmd_spickle_params2_arg2, + (prog_void *)&cmd_spickle_params2_arg3, NULL, }, }; @@ -746,8 +750,8 @@ parse_pgm_inst_t cmd_spickle_params2_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle_params2_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_params2_arg0, - (prog_void *)&cmd_spickle_params2_arg1_show, + (prog_void *)&cmd_spickle_params2_arg0, + (prog_void *)&cmd_spickle_params2_arg1_show, NULL, }, }; @@ -776,7 +780,7 @@ parse_pgm_inst_t cmd_test = { .data = NULL, /* 2nd arg of func */ .help_str = help_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_test_arg0, + (prog_void *)&cmd_test_arg0, NULL, }, }; diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c index f9c0b7d..a718970 100644 --- a/projects/microb2010/cobboard/cs.c +++ b/projects/microb2010/cobboard/cs.c @@ -95,10 +95,20 @@ static void do_cs(__attribute__((unused)) void *dummy) } #endif - if (cobboard.flags & DO_BD) { + if ((cobboard.flags & DO_BD) && (cobboard.flags & DO_POWER)) { bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs); bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs); bd_manage_from_cs(&cobboard.shovel.bd, &cobboard.shovel.cs); + + /* urgent case: stop power on blocking */ + if (cobboard.flags & DO_ERRBLOCKING) { + if (bd_get(&cobboard.left_spickle.bd) || + bd_get(&cobboard.right_spickle.bd) || + bd_get(&cobboard.shovel.bd)) { + printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); + cobboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); + } + } } if (cobboard.flags & DO_POWER) BRAKE_OFF(); @@ -154,7 +164,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&cobboard.left_spickle.bd); bd_set_speed_threshold(&cobboard.left_spickle.bd, 150); - bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 200); /* ---- CS right_spickle */ /* PID */ @@ -174,7 +184,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&cobboard.right_spickle.bd); bd_set_speed_threshold(&cobboard.right_spickle.bd, 150); - bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 200); /* ---- CS shovel */ /* PID */ @@ -200,7 +210,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&cobboard.shovel.bd); bd_set_speed_threshold(&cobboard.shovel.bd, 150); - bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 200); /* set them on (or not) !! */ cobboard.left_spickle.on = 0; diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c index 9de2d4c..8d86b49 100644 --- a/projects/microb2010/cobboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -93,7 +93,7 @@ static void i2c_send_status(void) /* status */ ans.mode = state_get_mode(); - ans.status = 0x55; /* TODO */ + ans.status = state_get_status(); ans.left_cobroller_speed = cobboard.left_cobroller_speed; ans.right_cobroller_speed = cobboard.right_cobroller_speed; diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index 24f14bc..1f4065b 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -115,16 +115,21 @@ void bootloader(void) void do_led_blink(__attribute__((unused)) void *dummy) { -#if 1 /* simple blink */ - static uint8_t a=0; + static uint8_t a = 0; - if(a) - LED1_ON(); - else - LED1_OFF(); - - a = !a; -#endif + if (cobboard.flags & DO_ERRBLOCKING) { + if (a & 1) + LED1_ON(); + else + LED1_OFF(); + } + else { + if (a & 4) + LED1_ON(); + else + LED1_OFF(); + } + a++; } static void main_timer_interrupt(void) @@ -153,7 +158,8 @@ int main(void) memset(&gen, 0, sizeof(gen)); memset(&cobboard, 0, sizeof(cobboard)); /* cs is enabled after arm_calibrate() */ - cobboard.flags = DO_ENCODERS | DO_POWER; // DO_BD + cobboard.flags = DO_ENCODERS | DO_POWER | DO_BD | + DO_ERRBLOCKING; /* UART */ uart_init(); diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index 0d07a9c..699a5c1 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -119,6 +119,7 @@ struct cobboard { #define DO_CS 2 #define DO_BD 4 #define DO_POWER 8 +#define DO_ERRBLOCKING 16 uint8_t flags; /* misc flags */ /* control systems */ diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 9d082ba..a16675d 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -58,6 +58,7 @@ static struct vt100 local_vt100; static volatile uint8_t state_mode; +static volatile uint8_t state_status; static uint8_t cob_count; /* short aliases */ @@ -72,13 +73,6 @@ static uint8_t cob_count; uint8_t state_debug = 0; -#if 0 -static void state_dump_sensors(void) -{ - STMCH_DEBUG("TODO\n"); -} -#endif - uint8_t state_get_cob_count(void) { return cob_count; @@ -125,11 +119,30 @@ static uint8_t state_no_cob_inside(void) !sensor_get(S_COB_INSIDE_R); } +/* pack/deploy spickles depending on mode */ +static void spickle_prepare(uint8_t side) +{ + if (cob_count >= 5) + spickle_pack(side); + else if (DEPLOY(side, state_mode) && !HARVEST(side, state_mode)) + spickle_mid(side); + else if (DEPLOY(side, state_mode) && HARVEST(side, state_mode)) + spickle_deploy(side); + else + spickle_pack(side); +} + /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode) { state_mode = mode; + /* preempt current action if not busy */ + if (state_status != I2C_COBBOARD_STATUS_LBUSY) + spickle_prepare(I2C_LEFT_SIDE); + if (state_status != I2C_COBBOARD_STATUS_RBUSY) + spickle_prepare(I2C_RIGHT_SIDE); + /* STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */ /* "r_deploy=%d r_harvest=%d eject=%d", */ /* __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), */ @@ -159,9 +172,19 @@ uint8_t state_get_mode(void) return state_mode; } +uint8_t state_get_status(void) +{ + return state_status; +} + /* harvest cobs from area */ static void state_do_harvest(uint8_t side) { + if (side == I2C_LEFT_SIDE) + state_status = I2C_COBBOARD_STATUS_LBUSY; + else + state_status = I2C_COBBOARD_STATUS_RBUSY; + /* if there is no cob, return */ if (state_cob_present(side)) return; @@ -178,13 +201,38 @@ static void state_do_harvest(uint8_t side) time_wait_ms(250); cobroller_on(side); + /* check that cob is correctly in place */ if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) { if (state_no_cob_inside()) { - printf_P(PSTR("NO COB\r\n")); + STMCH_DEBUG("no cob"); + return; + } + STMCH_DEBUG("bad cob state"); + + /* while cob is not correctly placed try to extract + * it as much as we can */ + while (state_cob_inside() == 0 && + state_no_cob_inside() == 0) { + uint8_t cpt = 0; + if (cpt == 0) + cobroller_reverse(side); + else if (cpt == 1) + cobroller_off(side); + else if (cpt == 2) + cobroller_on(side); + cpt ++; + cpt %= 3; + shovel_mid(); + time_wait_ms(250); + shovel_down(); + time_wait_ms(250); + } + + STMCH_DEBUG("cob removed"); + if (state_no_cob_inside()) { + STMCH_DEBUG("no cob"); return; } - printf_P(PSTR("BAD COB - press a key\r\n")); - while(!cmdline_keypressed()); } /* cob is inside, switch off roller */ @@ -208,10 +256,11 @@ static void state_do_harvest(uint8_t side) /* store it */ shovel_up(); - if (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) { - BRAKE_ON(); - printf_P(PSTR("BLOCKED\r\n")); - while(!cmdline_keypressed()); + while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) { + STMCH_DEBUG("shovel blocked"); + shovel_down(); + time_wait_ms(250); + shovel_up(); } state_debug_wait_key_pressed(); @@ -224,10 +273,11 @@ static void state_do_harvest(uint8_t side) shovel_down(); - if (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) { - BRAKE_ON(); - printf_P(PSTR("BLOCKED\r\n")); - while(!cmdline_keypressed()); + while (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) { + STMCH_DEBUG("shovel blocked"); + shovel_up(); + time_wait_ms(250); + shovel_down(); } STMCH_DEBUG("end"); @@ -236,6 +286,7 @@ static void state_do_harvest(uint8_t side) /* eject cobs */ static void state_do_eject(void) { + state_status = I2C_COBBOARD_STATUS_EJECT; cob_count = 0; shovel_mid(); servo_carry_open(); @@ -251,33 +302,19 @@ void state_machine(void) { while (state_want_exit() == 0) { + state_status = I2C_COBBOARD_STATUS_READY; + /* init */ if (INIT(state_mode)) { state_mode &= (~I2C_COBBOARD_MODE_INIT); state_init(); } + /* pack/deply spickles, enable/disable roller */ cobroller_off(I2C_LEFT_SIDE); cobroller_off(I2C_RIGHT_SIDE); - - /* pack/deply spickles, enable/disable roller */ - if (cob_count >= 5) - spickle_pack(I2C_LEFT_SIDE); - else if (L_DEPLOY(state_mode) && !L_HARVEST(state_mode)) - spickle_mid(I2C_LEFT_SIDE); - else if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) - spickle_deploy(I2C_LEFT_SIDE); - else - spickle_pack(I2C_LEFT_SIDE); - - if (cob_count >= 5) - spickle_pack(I2C_RIGHT_SIDE); - else if (R_DEPLOY(state_mode) && !R_HARVEST(state_mode)) - spickle_mid(I2C_RIGHT_SIDE); - else if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) - spickle_deploy(I2C_RIGHT_SIDE); - else - spickle_pack(I2C_RIGHT_SIDE); + spickle_prepare(I2C_LEFT_SIDE); + spickle_prepare(I2C_RIGHT_SIDE); /* harvest */ if (cob_count < 5) { @@ -305,4 +342,5 @@ void state_init(void) spickle_pack(I2C_RIGHT_SIDE); state_mode = 0; cob_count = 0; + state_status = I2C_COBBOARD_STATUS_READY; } diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h index 8dcb6c1..d9f95fc 100644 --- a/projects/microb2010/cobboard/state.h +++ b/projects/microb2010/cobboard/state.h @@ -27,6 +27,7 @@ int8_t state_set_mode(uint8_t mode); /* get current state */ uint8_t state_get_mode(void); +uint8_t state_get_status(void); uint8_t state_get_cob_count(void); diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 87f98c7..d6794af 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -52,7 +52,7 @@ struct i2c_cmd_hdr { struct i2c_cmd_led_control { struct i2c_cmd_hdr hdr; uint8_t led_num:7; - uint8_t state:1; + uint8_t state:1; }; /****/ @@ -115,9 +115,10 @@ struct i2c_ans_cobboard_status { /* mode type are defined above: I2C_COBBOARD_MODE_xxx */ uint8_t mode; -#define I2C_COBBOARD_STATUS_F_READY 0x00 -#define I2C_COBBOARD_STATUS_F_BUSY 0x01 -#define I2C_COBBOARD_STATUS_F_EXCPT 0x02 +#define I2C_COBBOARD_STATUS_READY 0x00 +#define I2C_COBBOARD_STATUS_LBUSY 0x01 +#define I2C_COBBOARD_STATUS_RBUSY 0x02 +#define I2C_COBBOARD_STATUS_EJECT 0x03 uint8_t status; uint8_t cob_count; diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index cdbeb6e..e5bb456 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -74,6 +74,10 @@ extern parse_pgm_inst_t cmd_cobboard_show; extern parse_pgm_inst_t cmd_cobboard_setmode1; extern parse_pgm_inst_t cmd_cobboard_setmode2; extern parse_pgm_inst_t cmd_cobboard_setmode3; +extern parse_pgm_inst_t cmd_ballboard_show; +extern parse_pgm_inst_t cmd_ballboard_setmode1; +extern parse_pgm_inst_t cmd_ballboard_setmode2; +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; @@ -82,6 +86,8 @@ extern parse_pgm_inst_t cmd_test; /* commands_traj.c */ extern parse_pgm_inst_t cmd_traj_speed; extern parse_pgm_inst_t cmd_traj_speed_show; +extern parse_pgm_inst_t cmd_traj_acc; +extern parse_pgm_inst_t cmd_traj_acc_show; extern parse_pgm_inst_t cmd_trajectory; extern parse_pgm_inst_t cmd_trajectory_show; extern parse_pgm_inst_t cmd_circle_coef; @@ -159,6 +165,10 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_cobboard_setmode1, (parse_pgm_inst_t *)&cmd_cobboard_setmode2, (parse_pgm_inst_t *)&cmd_cobboard_setmode3, + (parse_pgm_inst_t *)&cmd_ballboard_show, + (parse_pgm_inst_t *)&cmd_ballboard_setmode1, + (parse_pgm_inst_t *)&cmd_ballboard_setmode2, + (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_test, @@ -166,6 +176,8 @@ parse_pgm_ctx_t main_ctx[] = { /* commands_traj.c */ (parse_pgm_inst_t *)&cmd_traj_speed, (parse_pgm_inst_t *)&cmd_traj_speed_show, + (parse_pgm_inst_t *)&cmd_traj_acc, + (parse_pgm_inst_t *)&cmd_traj_acc_show, (parse_pgm_inst_t *)&cmd_trajectory, (parse_pgm_inst_t *)&cmd_trajectory_show, (parse_pgm_inst_t *)&cmd_circle_coef, diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index fe4fabb..a69208b 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1,6 +1,6 @@ /* * Copyright Droids Corporation (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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -81,29 +82,30 @@ static void cmd_event_parsed(void *parsed_result, void *data) u08 bit=0; struct cmd_event_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("all"))) { - bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS | - DO_BD | DO_TIMER | DO_POWER; + bit = 0xFF; if (!strcmp_P(res->arg2, PSTR("on"))) mainboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) mainboard.flags &= bit; else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), + printf_P(PSTR("encoders is %s\r\n"), (DO_ENCODERS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), + printf_P(PSTR("cs is %s\r\n"), (DO_CS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("rs is %s\r\n"), + printf_P(PSTR("rs is %s\r\n"), (DO_RS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("pos is %s\r\n"), + printf_P(PSTR("pos is %s\r\n"), (DO_POS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), + printf_P(PSTR("bd is %s\r\n"), (DO_BD & mainboard.flags) ? "on":"off"); - printf_P(PSTR("timer is %s\r\n"), + printf_P(PSTR("timer is %s\r\n"), (DO_TIMER & mainboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), + printf_P(PSTR("power is %s\r\n"), (DO_POWER & mainboard.flags) ? "on":"off"); + printf_P(PSTR("errblock is %s\r\n"), + (DO_ERRBLOCKING & mainboard.flags) ? "on":"off"); } return; } @@ -126,6 +128,8 @@ static void cmd_event_parsed(void *parsed_result, void *data) } else if (!strcmp_P(res->arg1, PSTR("power"))) bit = DO_POWER; + else if (!strcmp_P(res->arg1, PSTR("errblock"))) + bit = DO_ERRBLOCKING; if (!strcmp_P(res->arg2, PSTR("on"))) mainboard.flags |= bit; @@ -141,13 +145,13 @@ static void cmd_event_parsed(void *parsed_result, void *data) } mainboard.flags &= (~bit); } - printf_P(PSTR("%s is %s\r\n"), res->arg1, + printf_P(PSTR("%s is %s\r\n"), res->arg1, (bit & mainboard.flags) ? "on":"off"); } prog_char str_event_arg0[] = "event"; parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); -prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power"; +prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock"; parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); prog_char str_event_arg2[] = "on#off#show"; parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); @@ -158,9 +162,9 @@ parse_pgm_inst_t cmd_event = { .data = NULL, /* 2nd arg of func */ .help_str = help_event, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_event_arg0, - (prog_void *)&cmd_event_arg1, - (prog_void *)&cmd_event_arg2, + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, NULL, }, }; @@ -181,7 +185,7 @@ static void cmd_spi_test_parsed(void * parsed_result, void * data) printf("not implemented\n"); #else uint16_t i = 0, ret = 0, ret2 = 0; - + if (mainboard.flags & DO_ENCODERS) { printf_P(PSTR("Disable encoder event first\r\n")); return; @@ -211,7 +215,7 @@ parse_pgm_inst_t cmd_spi_test = { .data = NULL, /* 2nd arg of func */ .help_str = help_spi_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spi_test_arg0, + (prog_void *)&cmd_spi_test_arg0, NULL, }, }; @@ -251,8 +255,8 @@ parse_pgm_inst_t cmd_opponent = { .data = NULL, /* 2nd arg of func */ .help_str = help_opponent, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_opponent_arg0, - (prog_void *)&cmd_opponent_arg1, + (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg1, NULL, }, }; @@ -269,10 +273,10 @@ parse_pgm_inst_t cmd_opponent_set = { .data = NULL, /* 2nd arg of func */ .help_str = help_opponent_set, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg0, (prog_void *)&cmd_opponent_arg1_set, - (prog_void *)&cmd_opponent_arg2, - (prog_void *)&cmd_opponent_arg3, + (prog_void *)&cmd_opponent_arg2, + (prog_void *)&cmd_opponent_arg3, NULL, }, }; @@ -338,9 +342,9 @@ parse_pgm_inst_t cmd_start = { .data = NULL, /* 2nd arg of func */ .help_str = help_start, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_start_arg0, - (prog_void *)&cmd_start_color, - (prog_void *)&cmd_start_debug, + (prog_void *)&cmd_start_arg0, + (prog_void *)&cmd_start_color, + (prog_void *)&cmd_start_debug, NULL, }, }; @@ -358,7 +362,7 @@ struct cmd_interact_result { static void print_cs(void) { printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" " - "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), + "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), cs_get_consign(&mainboard.distance.cs), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_consign(&mainboard.distance.cs), @@ -371,7 +375,7 @@ static void print_cs(void) static void print_pos(void) { - printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), + printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), position_get_x_s16(&mainboard.pos), position_get_y_s16(&mainboard.pos), position_get_a_deg_s16(&mainboard.pos)); @@ -484,7 +488,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data) wait_ms(10); continue; } - + if (cmd == -1) { switch(c) { case '1': print ^= PRINT_POS; break; @@ -494,7 +498,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data) case '5': print ^= PRINT_TIME; break; case '6': print ^= PRINT_BLOCKING; break; - case 'q': + case 'q': if (mainboard.flags & DO_CS) strat_hardstop(); pwm_set_and_save(LEFT_PWM, 0); @@ -504,21 +508,21 @@ static void cmd_interact_parsed(void * parsed_result, void * data) pwm_set_and_save(LEFT_PWM, 0); pwm_set_and_save(RIGHT_PWM, 0); break; - default: + default: break; } } else { switch(cmd) { - case KEY_UP_ARR: + case KEY_UP_ARR: pwm_set_and_save(LEFT_PWM, 1200); pwm_set_and_save(RIGHT_PWM, 1200); break; - case KEY_LEFT_ARR: + case KEY_LEFT_ARR: pwm_set_and_save(LEFT_PWM, -1200); pwm_set_and_save(RIGHT_PWM, 1200); break; - case KEY_DOWN_ARR: + case KEY_DOWN_ARR: pwm_set_and_save(LEFT_PWM, -1200); pwm_set_and_save(RIGHT_PWM, -1200); break; @@ -541,7 +545,7 @@ parse_pgm_inst_t cmd_interact = { .data = NULL, /* 2nd arg of func */ .help_str = help_interact, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_interact_arg0, + (prog_void *)&cmd_interact_arg0, NULL, }, }; @@ -588,8 +592,8 @@ parse_pgm_inst_t cmd_color = { .data = NULL, /* 2nd arg of func */ .help_str = help_color, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_color_arg0, - (prog_void *)&cmd_color_color, + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, NULL, }, }; @@ -609,11 +613,11 @@ static void cmd_rs_parsed(void *parsed_result, void *data) { // struct cmd_rs_result *res = parsed_result; do { - printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_feedback(&mainboard.angle.cs), cs_get_out(&mainboard.angle.cs)); - printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.distance.cs), cs_get_filtered_feedback(&mainboard.distance.cs), cs_get_out(&mainboard.distance.cs)); @@ -634,8 +638,8 @@ parse_pgm_inst_t cmd_rs = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_arg0, - (prog_void *)&cmd_rs_arg1, + (prog_void *)&cmd_rs_arg0, + (prog_void *)&cmd_rs_arg1, NULL, }, }; @@ -668,7 +672,7 @@ parse_pgm_inst_t cmd_i2cdebug = { .data = NULL, /* 2nd arg of func */ .help_str = help_i2cdebug, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_i2cdebug_arg0, + (prog_void *)&cmd_i2cdebug_arg0, NULL, }, }; @@ -707,8 +711,8 @@ parse_pgm_inst_t cmd_cobboard_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_show_arg0, - (prog_void *)&cmd_cobboard_show_arg1, + (prog_void *)&cmd_cobboard_show_arg0, + (prog_void *)&cmd_cobboard_show_arg1, NULL, }, }; @@ -725,16 +729,12 @@ struct cmd_cobboard_setmode1_result { /* function called when cmd_cobboard_setmode1 is parsed successfully */ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) i2c_cobboard_mode_init(); else if (!strcmp_P(res->arg1, PSTR("eject"))) i2c_cobboard_mode_eject(); -#endif } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; @@ -748,8 +748,8 @@ parse_pgm_inst_t cmd_cobboard_setmode1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode1_arg0, - (prog_void *)&cmd_cobboard_setmode1_arg1, + (prog_void *)&cmd_cobboard_setmode1_arg0, + (prog_void *)&cmd_cobboard_setmode1_arg1, NULL, }, }; @@ -767,9 +767,6 @@ struct cmd_cobboard_setmode2_result { /* function called when cmd_cobboard_setmode2 is parsed successfully */ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; @@ -784,7 +781,6 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) i2c_cobboard_mode_harvest(side); else if (!strcmp_P(res->arg1, PSTR("pack"))) i2c_cobboard_mode_pack(side); -#endif } prog_char str_cobboard_setmode2_arg0[] = "cobboard"; @@ -800,9 +796,9 @@ parse_pgm_inst_t cmd_cobboard_setmode2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode2_arg0, - (prog_void *)&cmd_cobboard_setmode2_arg1, - (prog_void *)&cmd_cobboard_setmode2_arg2, + (prog_void *)&cmd_cobboard_setmode2_arg0, + (prog_void *)&cmd_cobboard_setmode2_arg1, + (prog_void *)&cmd_cobboard_setmode2_arg2, NULL, }, }; @@ -820,13 +816,9 @@ struct cmd_cobboard_setmode3_result { /* function called when cmd_cobboard_setmode3 is parsed successfully */ static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); -#endif } prog_char str_cobboard_setmode3_arg0[] = "cobboard"; @@ -841,9 +833,9 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode3_arg0, - (prog_void *)&cmd_cobboard_setmode3_arg1, - (prog_void *)&cmd_cobboard_setmode3_arg2, + (prog_void *)&cmd_cobboard_setmode3_arg0, + (prog_void *)&cmd_cobboard_setmode3_arg1, + (prog_void *)&cmd_cobboard_setmode3_arg2, NULL, }, }; @@ -880,8 +872,8 @@ parse_pgm_inst_t cmd_ballboard_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_show_arg0, - (prog_void *)&cmd_ballboard_show_arg1, + (prog_void *)&cmd_ballboard_show_arg0, + (prog_void *)&cmd_ballboard_show_arg1, NULL, }, }; @@ -898,9 +890,6 @@ struct cmd_ballboard_setmode1_result { /* function called when cmd_ballboard_setmode1 is parsed successfully */ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) @@ -913,7 +902,6 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* other commands */ -#endif } prog_char str_ballboard_setmode1_arg0[] = "ballboard"; @@ -927,8 +915,8 @@ parse_pgm_inst_t cmd_ballboard_setmode1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode1_arg0, - (prog_void *)&cmd_ballboard_setmode1_arg1, + (prog_void *)&cmd_ballboard_setmode1_arg0, + (prog_void *)&cmd_ballboard_setmode1_arg1, NULL, }, }; @@ -946,9 +934,6 @@ struct cmd_ballboard_setmode2_result { /* function called when cmd_ballboard_setmode2 is parsed successfully */ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode2_result *res = parsed_result; uint8_t mode = I2C_BALLBOARD_MODE_INIT; @@ -965,7 +950,6 @@ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) mode = I2C_BALLBOARD_MODE_TAKE_R_FORK; } i2c_ballboard_set_mode(mode); -#endif } prog_char str_ballboard_setmode2_arg0[] = "ballboard"; @@ -981,9 +965,9 @@ parse_pgm_inst_t cmd_ballboard_setmode2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode2_arg0, - (prog_void *)&cmd_ballboard_setmode2_arg1, - (prog_void *)&cmd_ballboard_setmode2_arg2, + (prog_void *)&cmd_ballboard_setmode2_arg0, + (prog_void *)&cmd_ballboard_setmode2_arg1, + (prog_void *)&cmd_ballboard_setmode2_arg2, NULL, }, }; @@ -1001,13 +985,9 @@ struct cmd_ballboard_setmode3_result { /* function called when cmd_ballboard_setmode3 is parsed successfully */ static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); -#endif } prog_char str_ballboard_setmode3_arg0[] = "ballboard"; @@ -1022,9 +1002,9 @@ parse_pgm_inst_t cmd_ballboard_setmode3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode3_arg0, - (prog_void *)&cmd_ballboard_setmode3_arg1, - (prog_void *)&cmd_ballboard_setmode3_arg2, + (prog_void *)&cmd_ballboard_setmode3_arg0, + (prog_void *)&cmd_ballboard_setmode3_arg1, + (prog_void *)&cmd_ballboard_setmode3_arg2, NULL, }, }; @@ -1063,8 +1043,8 @@ parse_pgm_inst_t cmd_servo_balls = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_balls, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_balls_arg0, - (prog_void *)&cmd_servo_balls_arg1, + (prog_void *)&cmd_servo_balls_arg0, + (prog_void *)&cmd_servo_balls_arg1, NULL, }, }; @@ -1220,8 +1200,16 @@ uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm, static void cmd_clitoid_parsed(void *parsed_result, void *data) { struct cmd_clitoid_result *res = parsed_result; - clitoid(res->alpha_deg, res->beta_deg, res->R_mm, - res->Vd, res->Amax, res->d_inter_mm); +/* clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */ +/* res->Vd, res->Amax, res->d_inter_mm); */ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + strat_set_speed(res->Vd, SPEED_ANGLE_FAST); + trajectory_clitoid(&mainboard.traj, x, y, a, 150., + res->alpha_deg, res->beta_deg, res->R_mm, + res->d_inter_mm); } prog_char str_clitoid_arg0[] = "clitoid"; @@ -1264,127 +1252,6 @@ parse_pgm_inst_t cmd_clitoid = { }, }; -////////////////////// - -// 500 -- 5 -// 400 -- 3 -#define TEST_SPEED 400 -#define TEST_ACC 3 - -static void line2line(double line1x1, double line1y1, - double line1x2, double line1y2, - double line2x1, double line2y1, - double line2x2, double line2y2, - double radius, double dist) -{ - uint8_t err; - double speed_d, speed_a; - double distance, angle; - double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1); - double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1); - - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - - strat_set_speed(TEST_SPEED, TEST_SPEED); - quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); - - circle_get_da_speed_from_radius(&mainboard.traj, radius, - &speed_d, &speed_a); - trajectory_line_abs(&mainboard.traj, - line1x1, line1y1, - line1x2, line1y2, 150.); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < - dist, TRAJ_FLAGS_NO_NEAR); - /* circle */ - strat_set_speed(speed_d, speed_a); - angle = line2_angle - line1_angle; - distance = angle * radius; - if (distance < 0) - distance = -distance; - angle = simple_modulo_2pi(angle); - angle = DEG(angle); - printf_P(PSTR("(%d,%d,%d) "), - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); - printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"), - distance, angle); - - /* take some margin on dist to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance + 250, angle); - - /* circle exit condition */ - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - strat_set_speed(500, 500); - printf_P(PSTR("(%d,%d,%d) "), - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); - printf_P(PSTR("line\r\n")); - trajectory_line_abs(&mainboard.traj, - line2x1, line2y1, - line2x2, line2y2, 150.); -} - -static void halfturn(double line1x1, double line1y1, - double line1x2, double line1y2, - double line2x1, double line2y1, - double line2x2, double line2y2, - double radius, double dist, double dir) -{ - uint8_t err; - double speed_d, speed_a; - double distance, angle; - - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - - strat_set_speed(TEST_SPEED, TEST_SPEED); - quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); - - circle_get_da_speed_from_radius(&mainboard.traj, radius, - &speed_d, &speed_a); - trajectory_line_abs(&mainboard.traj, - line1x1, line1y1, - line1x2, line1y2, 150.); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < - dist, TRAJ_FLAGS_NO_NEAR); - /* circle */ - strat_set_speed(speed_d, speed_a); - angle = dir * M_PI/2.; - distance = angle * radius; - if (distance < 0) - distance = -distance; - angle = simple_modulo_2pi(angle); - angle = DEG(angle); - - /* take some margin on dist to avoid deceleration */ - DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f", - distance, angle); - trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); - - /* circle exit condition */ - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - DEBUG(E_USER_STRAT, "miniline"); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) < - dist, TRAJ_FLAGS_NO_NEAR); - DEBUG(E_USER_STRAT, "circle2"); - /* take some margin on dist to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); - - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - strat_set_speed(500, 500); - DEBUG(E_USER_STRAT, "line"); - trajectory_line_abs(&mainboard.traj, - line2x1, line2y1, - line2x2, line2y2, 150.); -} - /**********************************************************/ /* Test */ @@ -1398,96 +1265,6 @@ struct cmd_test_result { /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { - // struct cmd_test_result *res = parsed_result; -#ifdef HOST_VERSION - strat_reset_pos(400, 400, 90); - mainboard.angle.on = 1; - mainboard.distance.on = 1; -#endif - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - while (!cmdline_keypressed()) { - /****** PASS1 */ - -#define DIST_HARD_TURN 260 -#define RADIUS_HARD_TURN 100 -#define DIST_EASY_TURN 190 -#define RADIUS_EASY_TURN 190 -#define DIST_HALF_TURN 225 -#define RADIUS_HALF_TURN 130 - - /* hard turn */ - line2line(375, 597, 375, 1847, - 375, 1847, 1050, 1472, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* easy left and easy right !*/ - line2line(825, 1596, 1050, 1472, - 1050, 1472, 1500, 1722, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1050, 1472, 1500, 1722, - 1500, 1722, 2175, 1347, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1500, 1722, 2175, 1347, - 2175, 1347, 2175, 847, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* half turns */ - halfturn(2175, 1347, 2175, 722, - 2625, 722, 2625, 1597, - RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); - halfturn(2625, 847, 2625, 1722, - 2175, 1722, 2175, 1097, - RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); - - /* easy turns */ - line2line(2175, 1597, 2175, 1097, - 2175, 1097, 1500, 722, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(2175, 1097, 1500, 722, - 1500, 722, 1050, 972, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1500, 722, 1050, 972, - 1050, 972, 375, 597, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(1050, 972, 375, 597, - 375, 597, 375, 1097, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /****** PASS2 */ - - /* easy turn */ - line2line(375, 597, 375, 1097, - 375, 1097, 1050, 1472, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(375, 1097, 1050, 1472, - 1050, 1472, 375, 1847, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* hard turn */ - line2line(1050, 1472, 375, 1847, - 375, 1847, 375, 1347, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* easy turn */ - line2line(375, 1847, 375, 1347, - 375, 1347, 1050, 972, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(375, 1347, 1050, 972, - 1050, 972, 375, 597, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* hard turn */ - line2line(1050, 972, 375, 597, - 375, 597, 375, 1847, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - } trajectory_hardstop(&mainboard.traj); } diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 7bf6057..8c59996 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -1,6 +1,6 @@ /* * Copyright Droids Corporation (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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -69,14 +69,14 @@ struct cmd_traj_speed_result { fixed_string_t arg0; fixed_string_t arg1; - uint16_t s; + float s; }; /* function called when cmd_traj_speed is parsed successfully */ static void cmd_traj_speed_parsed(void *parsed_result, void *data) { struct cmd_traj_speed_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("angle"))) { trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s); } @@ -85,7 +85,7 @@ static void cmd_traj_speed_parsed(void *parsed_result, void *data) } /* else it is a "show" */ - printf_P(PSTR("angle %u, distance %u\r\n"), + printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"), mainboard.traj.a_speed, mainboard.traj.d_speed); } @@ -94,7 +94,7 @@ prog_char str_traj_speed_arg0[] = "traj_speed"; parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0); prog_char str_traj_speed_arg1[] = "angle#distance"; parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1); -parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16); +parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT); prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager"; parse_pgm_inst_t cmd_traj_speed = { @@ -102,9 +102,9 @@ parse_pgm_inst_t cmd_traj_speed = { .data = NULL, /* 2nd arg of func */ .help_str = help_traj_speed, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_traj_speed_arg0, - (prog_void *)&cmd_traj_speed_arg1, - (prog_void *)&cmd_traj_speed_s, + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg1, + (prog_void *)&cmd_traj_speed_s, NULL, }, }; @@ -120,12 +120,76 @@ parse_pgm_inst_t cmd_traj_speed_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_traj_speed_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg0, (prog_void *)&cmd_traj_speed_show_arg, NULL, }, }; +/**********************************************************/ +/* Traj_Accs for trajectory_manager */ + +/* this structure is filled when cmd_traj_acc is parsed successfully */ +struct cmd_traj_acc_result { + fixed_string_t arg0; + fixed_string_t arg1; + float s; +}; + +/* function called when cmd_traj_acc is parsed successfully */ +static void cmd_traj_acc_parsed(void *parsed_result, void *data) +{ + struct cmd_traj_acc_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("angle"))) { + trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s); + } + else if (!strcmp_P(res->arg1, PSTR("distance"))) { + trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc); + } + /* else it is a "show" */ + + printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"), + mainboard.traj.a_acc, + mainboard.traj.d_acc); +} + +prog_char str_traj_acc_arg0[] = "traj_acc"; +parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0); +prog_char str_traj_acc_arg1[] = "angle#distance"; +parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1); +parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT); + +prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager"; +parse_pgm_inst_t cmd_traj_acc = { + .f = cmd_traj_acc_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_acc, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_acc_arg0, + (prog_void *)&cmd_traj_acc_arg1, + (prog_void *)&cmd_traj_acc_s, + NULL, + }, +}; + +/* show */ + +prog_char str_traj_acc_show_arg[] = "show"; +parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg); + +prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager"; +parse_pgm_inst_t cmd_traj_acc_show = { + .f = cmd_traj_acc_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_acc_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_acc_arg0, + (prog_void *)&cmd_traj_acc_show_arg, + NULL, + }, +}; + /**********************************************************/ /* circle coef configuration */ @@ -179,7 +243,7 @@ parse_pgm_inst_t cmd_circle_coef_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_circle_coef_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_circle_coef_arg0, + (prog_void *)&cmd_circle_coef_arg0, (prog_void *)&cmd_circle_coef_show_arg, NULL, }, @@ -202,7 +266,7 @@ struct cmd_trajectory_result { static void cmd_trajectory_parsed(void * parsed_result, void * data) { struct cmd_trajectory_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("set"))) { trajectory_set_windows(&mainboard.traj, res->d_win, res->a_win, res->a_start); @@ -226,11 +290,11 @@ parse_pgm_inst_t cmd_trajectory = { .data = NULL, /* 2nd arg of func */ .help_str = help_trajectory, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_trajectory_arg0, - (prog_void *)&cmd_trajectory_arg1, - (prog_void *)&cmd_trajectory_d, - (prog_void *)&cmd_trajectory_a, - (prog_void *)&cmd_trajectory_as, + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg1, + (prog_void *)&cmd_trajectory_d, + (prog_void *)&cmd_trajectory_a, + (prog_void *)&cmd_trajectory_as, NULL, }, }; @@ -246,7 +310,7 @@ parse_pgm_inst_t cmd_trajectory_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_trajectory_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg0, (prog_void *)&cmd_trajectory_show_arg, NULL, }, @@ -272,9 +336,9 @@ static void cmd_rs_gains_parsed(void * parsed_result, void * data) struct cmd_rs_gains_result * res = parsed_result; if (!strcmp_P(res->arg1, PSTR("set"))) { - rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, + rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, LEFT_ENCODER, res->left); // en augmentant on tourne à gauche - rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, + rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, RIGHT_ENCODER, res->right); //en augmentant on tourne à droite } printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"), @@ -295,10 +359,10 @@ parse_pgm_inst_t cmd_rs_gains = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs_gains, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_gains_arg0, - (prog_void *)&cmd_rs_gains_arg1, - (prog_void *)&cmd_rs_gains_l, - (prog_void *)&cmd_rs_gains_r, + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg1, + (prog_void *)&cmd_rs_gains_l, + (prog_void *)&cmd_rs_gains_r, NULL, }, }; @@ -314,7 +378,7 @@ parse_pgm_inst_t cmd_rs_gains_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs_gains_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg0, (prog_void *)&cmd_rs_gains_show_arg, NULL, }, @@ -353,9 +417,9 @@ parse_pgm_inst_t cmd_track = { .data = NULL, /* 2nd arg of func */ .help_str = help_track, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_track_arg0, - (prog_void *)&cmd_track_arg1, - (prog_void *)&cmd_track_val, + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg1, + (prog_void *)&cmd_track_val, NULL, }, }; @@ -371,7 +435,7 @@ parse_pgm_inst_t cmd_track_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_track_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg0, (prog_void *)&cmd_track_show_arg, NULL, }, @@ -400,7 +464,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) { struct cmd_pt_list_result * res = parsed_result; uint8_t i, why=0; - + if (!strcmp_P(res->arg1, PSTR("append"))) { res->arg2 = pt_list_len; } @@ -419,7 +483,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) printf_P(PSTR("List is too large\r\n")); return; } - memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], + memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], PT_LIST_SIZE-1-res->arg2); pt_list[res->arg2].x = res->arg3; pt_list[res->arg2].y = res->arg4; @@ -434,14 +498,14 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) printf_P(PSTR("Index too large\r\n")); return; } - memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], + memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point)); pt_list_len--; } else if (!strcmp_P(res->arg1, PSTR("reset"))) { pt_list_len = 0; } - + /* else it is a "show" or a "start" */ if (pt_list_len == 0) { printf_P(PSTR("List empty\r\n")); @@ -493,11 +557,11 @@ parse_pgm_inst_t cmd_pt_list = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, - (prog_void *)&cmd_pt_list_arg1, - (prog_void *)&cmd_pt_list_arg2, - (prog_void *)&cmd_pt_list_arg3, - (prog_void *)&cmd_pt_list_arg4, + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1, + (prog_void *)&cmd_pt_list_arg2, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, NULL, }, }; @@ -513,10 +577,10 @@ parse_pgm_inst_t cmd_pt_list_append = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list_append, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, - (prog_void *)&cmd_pt_list_arg1_append, - (prog_void *)&cmd_pt_list_arg3, - (prog_void *)&cmd_pt_list_arg4, + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1_append, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, NULL, }, }; @@ -532,8 +596,8 @@ parse_pgm_inst_t cmd_pt_list_del = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list_del, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, - (prog_void *)&cmd_pt_list_del_arg, + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_del_arg, (prog_void *)&cmd_pt_list_arg2, NULL, }, @@ -549,7 +613,7 @@ parse_pgm_inst_t cmd_pt_list_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg0, (prog_void *)&cmd_pt_list_show_arg, NULL, }, @@ -654,9 +718,9 @@ parse_pgm_inst_t cmd_goto1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_goto1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_goto_arg0, - (prog_void *)&cmd_goto_arg1_a, - (prog_void *)&cmd_goto_arg2, + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_a, + (prog_void *)&cmd_goto_arg2, NULL, }, }; @@ -672,10 +736,10 @@ parse_pgm_inst_t cmd_goto2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_goto2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_goto_arg0, - (prog_void *)&cmd_goto_arg1_b, + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_b, (prog_void *)&cmd_goto_arg2, - (prog_void *)&cmd_goto_arg3, + (prog_void *)&cmd_goto_arg3, NULL, }, }; @@ -780,7 +844,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) } /* else it's just a "show" */ - printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), + printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), position_get_x_double(&mainboard.pos), position_get_y_double(&mainboard.pos), DEG(position_get_a_rad_double(&mainboard.pos))); @@ -797,8 +861,8 @@ parse_pgm_inst_t cmd_position = { .data = NULL, /* 2nd arg of func */ .help_str = help_position, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_position_arg0, - (prog_void *)&cmd_position_arg1, + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1, NULL, }, }; @@ -816,11 +880,11 @@ parse_pgm_inst_t cmd_position_set = { .data = NULL, /* 2nd arg of func */ .help_str = help_position_set, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_position_arg0, - (prog_void *)&cmd_position_arg1_set, - (prog_void *)&cmd_position_arg2, - (prog_void *)&cmd_position_arg3, - (prog_void *)&cmd_position_arg4, + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1_set, + (prog_void *)&cmd_position_arg2, + (prog_void *)&cmd_position_arg3, + (prog_void *)&cmd_position_arg4, NULL, }, }; @@ -858,8 +922,8 @@ parse_pgm_inst_t cmd_strat_infos = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_infos, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_infos_arg0, - (prog_void *)&cmd_strat_infos_arg1, + (prog_void *)&cmd_strat_infos_arg0, + (prog_void *)&cmd_strat_infos_arg1, NULL, }, }; @@ -893,8 +957,8 @@ parse_pgm_inst_t cmd_strat_conf = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_conf, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf_arg0, - (prog_void *)&cmd_strat_conf_arg1, + (prog_void *)&cmd_strat_conf_arg0, + (prog_void *)&cmd_strat_conf_arg1, NULL, }, }; @@ -919,7 +983,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) on = 1; else on = 0; - + #if 0 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc"))) bit = STRAT_CONF_ONLY_ONE_ON_DISC; @@ -962,9 +1026,9 @@ parse_pgm_inst_t cmd_strat_conf2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_conf2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf2_arg0, - (prog_void *)&cmd_strat_conf2_arg1, - (prog_void *)&cmd_strat_conf2_arg2, + (prog_void *)&cmd_strat_conf2_arg0, + (prog_void *)&cmd_strat_conf2_arg1, + (prog_void *)&cmd_strat_conf2_arg2, NULL, }, }; @@ -1028,9 +1092,9 @@ parse_pgm_inst_t cmd_strat_conf3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_conf3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf3_arg0, - (prog_void *)&cmd_strat_conf3_arg1, - (prog_void *)&cmd_strat_conf3_arg2, + (prog_void *)&cmd_strat_conf3_arg0, + (prog_void *)&cmd_strat_conf3_arg1, + (prog_void *)&cmd_strat_conf3_arg2, NULL, }, }; @@ -1039,6 +1103,224 @@ parse_pgm_inst_t cmd_strat_conf3 = { /**********************************************************/ /* Subtraj */ +////////////////////// + +// 500 -- 5 +// 400 -- 3 +#define TEST_SPEED 400 +#define TEST_ACC 3 + +static void line2line(double line1x1, double line1y1, + double line1x2, double line1y2, + double line2x1, double line2y1, + double line2x2, double line2y2, + double radius, double dist) +{ + uint8_t err; + double speed_d, speed_a; + double distance, angle; + double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1); + double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1); + + printf_P(PSTR("%s()\r\n"), __FUNCTION__); + + strat_set_speed(TEST_SPEED, TEST_SPEED); + quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); + + circle_get_da_speed_from_radius(&mainboard.traj, radius, + &speed_d, &speed_a); + trajectory_line_abs(&mainboard.traj, + line1x1, line1y1, + line1x2, line1y2, 150.); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < + dist, TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(speed_d, speed_a); + angle = line2_angle - line1_angle; + distance = angle * radius; + if (distance < 0) + distance = -distance; + angle = simple_modulo_2pi(angle); + angle = DEG(angle); + printf_P(PSTR("(%d,%d,%d) "), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"), + distance, angle); + + /* take some margin on dist to avoid deceleration */ + trajectory_d_a_rel(&mainboard.traj, distance + 250, angle); + + /* circle exit condition */ + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + printf_P(PSTR("(%d,%d,%d) "), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + printf_P(PSTR("line\r\n")); + trajectory_line_abs(&mainboard.traj, + line2x1, line2y1, + line2x2, line2y2, 150.); +} + +static void halfturn(double line1x1, double line1y1, + double line1x2, double line1y2, + double line2x1, double line2y1, + double line2x2, double line2y2, + double radius, double dist, double dir) +{ + uint8_t err; + double speed_d, speed_a; + double distance, angle; + + printf_P(PSTR("%s()\r\n"), __FUNCTION__); + + strat_set_speed(TEST_SPEED, TEST_SPEED); + quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); + + circle_get_da_speed_from_radius(&mainboard.traj, radius, + &speed_d, &speed_a); + trajectory_line_abs(&mainboard.traj, + line1x1, line1y1, + line1x2, line1y2, 150.); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < + dist, TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(speed_d, speed_a); + angle = dir * M_PI/2.; + distance = angle * radius; + if (distance < 0) + distance = -distance; + angle = simple_modulo_2pi(angle); + angle = DEG(angle); + + /* take some margin on dist to avoid deceleration */ + DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f", + distance, angle); + trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); + + /* circle exit condition */ + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_NO_NEAR); + + DEBUG(E_USER_STRAT, "miniline"); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) < + dist, TRAJ_FLAGS_NO_NEAR); + DEBUG(E_USER_STRAT, "circle2"); + /* take some margin on dist to avoid deceleration */ + trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); + + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + DEBUG(E_USER_STRAT, "line"); + trajectory_line_abs(&mainboard.traj, + line2x1, line2y1, + line2x2, line2y2, 150.); +} + + +/* function called when cmd_test is parsed successfully */ +static void subtraj_test(void) +{ +#ifdef HOST_VERSION + strat_reset_pos(400, 400, 90); + mainboard.angle.on = 1; + mainboard.distance.on = 1; +#endif + printf_P(PSTR("%s()\r\n"), __FUNCTION__); + while (!cmdline_keypressed()) { + /****** PASS1 */ + +#define DIST_HARD_TURN 260 +#define RADIUS_HARD_TURN 100 +#define DIST_EASY_TURN 190 +#define RADIUS_EASY_TURN 190 +#define DIST_HALF_TURN 225 +#define RADIUS_HALF_TURN 130 + + /* hard turn */ + line2line(375, 597, 375, 1847, + 375, 1847, 1050, 1472, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* easy left and easy right !*/ + line2line(825, 1596, 1050, 1472, + 1050, 1472, 1500, 1722, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(1050, 1472, 1500, 1722, + 1500, 1722, 2175, 1347, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(1500, 1722, 2175, 1347, + 2175, 1347, 2175, 847, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* half turns */ + halfturn(2175, 1347, 2175, 722, + 2625, 722, 2625, 1597, + RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); + halfturn(2625, 847, 2625, 1722, + 2175, 1722, 2175, 1097, + RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); + + /* easy turns */ + line2line(2175, 1597, 2175, 1097, + 2175, 1097, 1500, 722, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(2175, 1097, 1500, 722, + 1500, 722, 1050, 972, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(1500, 722, 1050, 972, + 1050, 972, 375, 597, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* hard turn */ + line2line(1050, 972, 375, 597, + 375, 597, 375, 1097, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /****** PASS2 */ + + /* easy turn */ + line2line(375, 597, 375, 1097, + 375, 1097, 1050, 1472, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* hard turn */ + line2line(375, 1097, 1050, 1472, + 1050, 1472, 375, 1847, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* hard turn */ + line2line(1050, 1472, 375, 1847, + 375, 1847, 375, 1347, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* easy turn */ + line2line(375, 1847, 375, 1347, + 375, 1347, 1050, 972, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* hard turn */ + line2line(375, 1347, 1050, 972, + 1050, 972, 375, 597, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* hard turn */ + line2line(1050, 972, 375, 597, + 375, 597, 375, 1847, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + } + trajectory_hardstop(&mainboard.traj); +} + + /* this structure is filled when cmd_subtraj is parsed successfully */ struct cmd_subtraj_result { fixed_string_t arg0; @@ -1052,9 +1334,11 @@ struct cmd_subtraj_result { /* function called when cmd_subtraj is parsed successfully */ static void cmd_subtraj_parsed(void *parsed_result, void *data) { -/* struct cmd_subtraj_result *res = parsed_result; */ + struct cmd_subtraj_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("test"))) + subtraj_test(); - printf_P(PSTR("TODO\r\n")); trajectory_hardstop(&mainboard.traj); } @@ -1073,12 +1357,13 @@ parse_pgm_inst_t cmd_subtraj = { .data = NULL, /* 2nd arg of func */ .help_str = help_subtraj, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_subtraj_arg0, - (prog_void *)&cmd_subtraj_arg1, - (prog_void *)&cmd_subtraj_arg2, - (prog_void *)&cmd_subtraj_arg3, - (prog_void *)&cmd_subtraj_arg4, - (prog_void *)&cmd_subtraj_arg5, + (prog_void *)&cmd_subtraj_arg0, + (prog_void *)&cmd_subtraj_arg1, + (prog_void *)&cmd_subtraj_arg2, + (prog_void *)&cmd_subtraj_arg3, + (prog_void *)&cmd_subtraj_arg4, + (prog_void *)&cmd_subtraj_arg5, NULL, }, }; + diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 905d533..b8d6e08 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -126,12 +126,19 @@ static void do_cs(void *dummy) * compensation) */ position_manage(&mainboard.pos); } - if (mainboard.flags & DO_BD) { + if ((mainboard.flags & DO_BD) && (mainboard.flags & DO_POWER)) { bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs); bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs); #ifndef HOST_VERSION bd_manage_from_cs(&mainboard.left_cobroller.bd, &mainboard.left_cobroller.cs); bd_manage_from_cs(&mainboard.right_cobroller.bd, &mainboard.right_cobroller.cs); + if (mainboard.flags & DO_ERRBLOCKING) { + if (bd_get(&mainboard.left_cobroller.bd) || + bd_get(&mainboard.left_cobroller.bd)) { + printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); + mainboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); + } + } #endif } #ifndef HOST_VERSION @@ -226,7 +233,7 @@ void microb_cs_init(void) &mainboard.angle.cs); trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos); trajectory_set_speed(&mainboard.traj, SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* d, a */ - trajectory_set_speed(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */ + trajectory_set_acc(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */ /* distance window, angle window, angle start */ trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.); diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index ad8cff2..64fa521 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -1,6 +1,9 @@ -import math, sys, time, os, random +import math, sys, time, os, random, re from visual import * +FLOAT = "([-+]?[0-9]*\.?[0-9]+)" +INT = "([-+]?[0-9][0-9]*)" + AREA_X = 3000. AREA_Y = 2100. @@ -16,11 +19,11 @@ scene.autoscale=0 # all positions of robot every 5ms save_pos = [] -robot = box(pos = (0, 0, ROBOT_HEIGHT/2), - size = (250,320,ROBOT_HEIGHT), - color = (0.3, 0.3, 0.3) ) +robot = box(color=(0.4, 0.4, 0.4)) +lspickle = box(color=(0.4, 0.4, 0.4)) +rspickle = box(color=(0.4, 0.4, 0.4)) -last_pos = robot.pos.x, robot.pos.y, robot.pos.z +last_pos = (0.,0.,0.) hcenter_line = curve() hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)] vcenter_line = curve() @@ -50,6 +53,11 @@ def square(sz): sq1 = square(250) sq2 = square(500) +robot_x = 0. +robot_y = 0. +robot_a = 0. +robot_lspickle = 0 +robot_rspickle = 0 robot_trail = curve() robot_trail_list = [] max_trail = 500 @@ -181,6 +189,21 @@ def init_waypoints(): def toggle_obj_disp(): global area_objects + """ + if area_objects == []: + c = sphere(radius=5, color=(0., 0.,1.), + pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5)) + area_objects.append(c) + c = sphere(radius=5, color=(0., 0.,1.), + pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5)) + area_objects.append(c) + c = sphere(radius=5, color=(0., 0.,1.), + pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5)) + area_objects.append(c) + c = sphere(radius=5, color=(0., 0.,1.), + pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5)) + area_objects.append(c) +""" if area_objects == []: i = 0 j = 0 @@ -223,18 +246,32 @@ def toggle_obj_disp(): o.visible = 1 -def set_robot(x, y, a): +def set_robot(): global robot, last_pos, robot_trail, robot_trail_list - global save_pos + global save_pos, robot_x, robot_y, robot_a - robot.pos = (x - AREA_X/2, y - AREA_Y/2, ROBOT_HEIGHT/2) - robot.axis = (math.cos(a*math.pi/180), - math.sin(a*math.pi/180), - 0) + axis = (math.cos(robot_a*math.pi/180), + math.sin(robot_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*70) * math.cos((robot_a-90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_lspickle*70) * math.sin((robot_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*70) * math.cos((robot_a+90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_rspickle*70) * math.sin((robot_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, a)) + save_pos.append((robot.pos.x, robot.pos, robot_a)) pos = robot.pos.x, robot.pos.y, 0.3 if pos != last_pos: @@ -272,11 +309,30 @@ while True: fw = open("/tmp/.robot_dis2sim", "w", 0) while True: l = fr.readline() - try: - x,y,a = map(lambda x:int(x), l[:-1].split(" ")) - set_robot(x,y,a) - except ValueError: - pass + m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l) + if m: + robot_x = int(m.groups()[0]) + robot_y = int(m.groups()[1]) + robot_a = int(m.groups()[2]) + set_robot() + m = re.match("ballboard=%s"%(INT), l) + if m: + print int(m.groups()[0]) + m = re.match("cobboard=%s"%(INT), l) + if m: + mode = int(m.groups()[0]) + if (mode & 0x01) == 0: + robot_lspickle = 0 + elif (mode & 0x02) == 0: + robot_lspickle = 1 + else: + robot_lspickle = 2 + if (mode & 0x04) == 0: + robot_rspickle = 0 + elif (mode & 0x08) == 0: + robot_rspickle = 1 + else: + robot_rspickle = 2 if scene.kb.keys == 0: continue diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index 4e5c174..a4aa028 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -53,6 +53,9 @@ #include "main.h" #include "sensor.h" #include "i2c_protocol.h" +#ifdef HOST_VERSION +#include "robotsim.h" +#endif #define I2C_STATE_MAX 4 @@ -321,7 +324,7 @@ static int8_t i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size) { #ifdef HOST_VERSION - return 0; + return robotsim_i2c(addr, buf, size); #else uint8_t flags; microseconds us = time_get_us2(); @@ -458,6 +461,6 @@ int8_t i2c_ballboard_set_mode(uint8_t mode) struct i2c_cmd_ballboard_set_mode buf; buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE; buf.mode = mode; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); } diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 88d4f97..98d3749 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -133,9 +133,21 @@ void do_time_monitor(void *dummy) void do_led_blink(void *dummy) { -#if 1 /* simple blink */ - LED1_TOGGLE(); -#endif + static uint8_t a = 0; + + if (mainboard.flags & DO_ERRBLOCKING) { + if (a & 1) + LED1_ON(); + else + LED1_OFF(); + } + else { + if (a & 4) + LED1_ON(); + else + LED1_OFF(); + } + a++; } static void main_timer_interrupt(void) @@ -171,7 +183,7 @@ int main(void) memset(&gen, 0, sizeof(gen)); memset(&mainboard, 0, sizeof(mainboard)); mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | - DO_POS | DO_POWER | DO_BD; + DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING; /* UART */ uart_init(); @@ -262,13 +274,13 @@ int main(void) /* all cs management */ microb_cs_init(); + /* TIME */ + time_init(TIME_PRIO); + #ifndef HOST_VERSION /* sensors, will also init hardware adc */ sensor_init(); - /* TIME */ - time_init(TIME_PRIO); - /* start i2c slave polling */ scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL, 8000L / SCHEDULER_UNIT, I2C_POLL_PRIO); @@ -281,7 +293,7 @@ int main(void) /* strat-related event */ scheduler_add_periodical_event_priority(strat_event, NULL, - 25000L / SCHEDULER_UNIT, + 10000L / SCHEDULER_UNIT, STRAT_PRIO); #ifndef HOST_VERSION diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 48655eb..6e3f8ad 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -168,6 +168,7 @@ struct mainboard { #define DO_BD 16 #define DO_TIMER 32 #define DO_POWER 64 +#define DO_ERRBLOCKING 128 uint8_t flags; /* misc flags */ /* control systems */ diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index d0aa3ae..5fc2e37 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -48,6 +48,7 @@ #include #include +#include "../common/i2c_commands.h" #include "strat.h" #include "strat_utils.h" #include "main.h" @@ -67,7 +68,7 @@ void robotsim_dump(void) char buf[BUFSIZ]; int len; - len = snprintf(buf, sizeof(buf), "%d %d %d\n", + 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)); @@ -76,6 +77,81 @@ void robotsim_dump(void) hostsim_unlock(); } +static int8_t +robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd) +{ + char buf[BUFSIZ]; + int len; + + ballboard.mode = cmd->mode; + len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode); + hostsim_lock(); + write(fdw, buf, len); + hostsim_unlock(); + return 0; +} + +static int8_t +robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) +{ + char buf[BUFSIZ]; + int len; + + cobboard.mode = cmd->mode; + len = snprintf(buf, sizeof(buf), "cobboard=%d\n", cmd->mode); + hostsim_lock(); + write(fdw, buf, len); + hostsim_unlock(); + return 0; +} + +static int8_t +robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size) +{ + void *void_cmd = buf; + + switch (buf[0]) { + case I2C_CMD_BALLBOARD_SET_MODE: + { + struct i2c_cmd_ballboard_set_mode *cmd = void_cmd; + robotsim_i2c_ballboard_set_mode(cmd); + break; + } + + default: + break; + } + return 0; +} + +static int8_t +robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size) +{ + void *void_cmd = buf; + + switch (buf[0]) { + case I2C_CMD_COBBOARD_SET_MODE: + { + struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; + robotsim_i2c_cobboard_set_mode(cmd); + break; + } + default: + break; + } + return 0; +} + +int8_t +robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size) +{ + if (addr == I2C_BALLBOARD_ADDR) + return robotsim_i2c_ballboard(addr, buf, size); + else if (addr == I2C_COBBOARD_ADDR) + return robotsim_i2c_cobboard(addr, buf, size); + return 0; +} + /* must be called periodically */ void robotsim_update(void) { diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h index 8777de8..a9316b9 100644 --- a/projects/microb2010/mainboard/robotsim.h +++ b/projects/microb2010/mainboard/robotsim.h @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * 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 @@ -19,6 +19,7 @@ * */ +int8_t robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size); void robotsim_update(void); void robotsim_pwm(void *arg, int32_t val); int32_t robotsim_encoder_get(void *arg); diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 6743d37..97d159c 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids, 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 @@ -17,7 +17,7 @@ * * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -70,7 +70,10 @@ struct strat_infos strat_infos = { .conf = { .flags = 0, }, - + /* status */ + .status = { + .flags = 0, + }, }; /*************************************************************/ @@ -150,7 +153,7 @@ void strat_init(void) interrupt_traj_reset(); /* used in strat_base for END_TIMER */ - mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | + mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | DO_POS | DO_BD | DO_TIMER | DO_POWER; } @@ -174,6 +177,17 @@ void strat_exit(void) /* called periodically */ void strat_event(void *dummy) { +#if 0 + /* pack or deploy spickle */ + if (strat_infos.status.flags & STRAT_STATUS_LHARVEST) { + if (sensor_get(S_LCOB_PRESENT)) { + if (sensor_get(S_LCOB_WHITE)) + i2c_ballboard_set_mode(); + else + ; + } + } +#endif /* limit speed when opponent is close */ strat_limit_speed(); } @@ -197,7 +211,7 @@ uint8_t strat_main(void) { uint8_t err; - /* do static cols + first temple */ + /* */ err = strat_beginning(); return err; diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 53d9192..d852396 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -83,23 +83,30 @@ /* strat infos structures */ -struct bbox { +struct strat_bbox { int32_t x1; int32_t y1; int32_t x2; int32_t y2; }; -struct conf { +struct strat_conf { #define STRAT_CONF_XXX 0x01 uint8_t flags; }; +struct strat_status { +#define STRAT_STATUS_LHARVEST 0x01 +#define STRAT_STATUS_RHARVEST 0x02 + uint8_t flags; +}; + /* all infos related to strat */ struct strat_infos { uint8_t dump_enabled; - struct conf conf; - struct bbox area_bbox; + struct strat_conf conf; + struct strat_bbox area_bbox; + struct strat_status status; }; extern struct strat_infos strat_infos; -- 2.20.1