X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_base.c;h=90762a635d72a7066f8b0011eeaa04e4517fa3f6;hp=21d526a9415e08d4748ce76a068c960ad084f761;hb=HEAD;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index 21d526a..90762a6 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology (2009) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -31,13 +31,14 @@ #include #include #include -#include +#include #include #include #include #include #include +#include #include #include #include @@ -58,10 +59,14 @@ #include "strat.h" #include "sensor.h" #include "i2c_protocol.h" +#include "robotsim.h" /* true if we want to interrupt a trajectory */ static uint8_t traj_intr=0; +/* to debug state of strat_obstacle() */ +static uint8_t strat_obstacle_debug = 0; + /* filled when a END_OBSTACLE is returned */ struct opponent_obstacle opponent_obstacle; @@ -100,8 +105,10 @@ const char *get_err(uint8_t err) return "END_UNKNOWN"; } -void strat_hardstop(void) +void strat_hardstop(void) { + DEBUG(E_USER_STRAT, "strat_hardstop"); + trajectory_hardstop(&mainboard.traj); pid_reset(&mainboard.angle.pid); pid_reset(&mainboard.distance.pid); @@ -126,27 +133,6 @@ uint8_t strat_goto_xy_force(int16_t x, int16_t y) { uint8_t i, err; -#ifdef HOMOLOGATION - int8_t serr; - uint8_t hardstop = 0; - microseconds us = time_get_us2(); - int16_t opp_a, opp_d, opp_x, opp_y; - - while (1) { - serr = get_opponent_xyda(&opp_x, &opp_y, - &opp_d, &opp_a); - if (serr == -1) - break; - if (opp_d < 600) - break; - if (hardstop == 0) { - strat_hardstop(); - hardstop = 1; - } - if ((time_get_us2() - us) > 3000000L) - break; - } -#endif for (i=0; i<3; i++) { trajectory_goto_xy_abs(&mainboard.traj, x, y); err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); @@ -160,20 +146,39 @@ uint8_t strat_goto_xy_force(int16_t x, int16_t y) return END_BLOCKING; } -/* reset position */ -void strat_reset_pos(int16_t x, int16_t y, int16_t a) +/* reset position */ +void strat_reset_pos(int16_t x, int16_t y, double a) { - int16_t posx = position_get_x_s16(&mainboard.pos); - int16_t posy = position_get_y_s16(&mainboard.pos); - int16_t posa = position_get_a_deg_s16(&mainboard.pos); + double posx = position_get_x_double(&mainboard.pos); + double posy = position_get_y_double(&mainboard.pos); + double posa = position_get_a_rad_double(&mainboard.pos); if (x == DO_NOT_SET_POS) x = posx; if (y == DO_NOT_SET_POS) y = posy; if (a == DO_NOT_SET_POS) - a = posa; - + a = DEG(posa); + + /* some issues with blocking on simulator */ +#ifdef HOST_VERSION + if (x == ROBOT_HALF_LENGTH_REAR) + x = ROBOT_HALF_LENGTH_REAR + 10; + if (x == AREA_X - ROBOT_HALF_LENGTH_REAR) + x = AREA_X - ROBOT_HALF_LENGTH_REAR - 10; + if (y == ROBOT_HALF_LENGTH_REAR) + y = ROBOT_HALF_LENGTH_REAR + 10; + if (y == AREA_Y - ROBOT_HALF_LENGTH_REAR) + y = AREA_Y - ROBOT_HALF_LENGTH_REAR - 10; + if (x == ROBOT_HALF_LENGTH_FRONT) + x = ROBOT_HALF_LENGTH_FRONT + 10; + if (x == AREA_X - ROBOT_HALF_LENGTH_FRONT) + x = AREA_X - ROBOT_HALF_LENGTH_FRONT - 10; + if (y == ROBOT_HALF_LENGTH_FRONT) + y = ROBOT_HALF_LENGTH_FRONT + 10; + if (y == AREA_Y - ROBOT_HALF_LENGTH_FRONT) + y = AREA_Y - ROBOT_HALF_LENGTH_FRONT - 10; +#endif DEBUG(E_USER_STRAT, "reset pos (%s%s%s)", x == DO_NOT_SET_POS ? "" : "x", y == DO_NOT_SET_POS ? "" : "y", @@ -182,7 +187,7 @@ void strat_reset_pos(int16_t x, int16_t y, int16_t a) DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__); } -/* +/* * decrease gain on angle PID, and go forward until we reach the * border. */ @@ -191,47 +196,48 @@ uint8_t strat_calib(int16_t dist, uint8_t flags) int32_t p = pid_get_gain_P(&mainboard.angle.pid); int32_t i = pid_get_gain_I(&mainboard.angle.pid); int32_t d = pid_get_gain_D(&mainboard.angle.pid); + int32_t max_in = pid_get_max_in(&mainboard.angle.pid); + int32_t max_i = pid_get_max_I(&mainboard.angle.pid); + int32_t max_out = pid_get_max_out(&mainboard.angle.pid); + uint32_t i_thres = mainboard.distance.bd.i_thres; + int32_t k1 = mainboard.distance.bd.k1; + int32_t k2 = mainboard.distance.bd.k2; + uint16_t cpt_thres = mainboard.distance.bd.cpt_thres; + uint8_t err; - pid_set_gains(&mainboard.angle.pid, 150, 0, 2000); + /* go with a lower angle pid, and with a sensible blocking + * detection */ + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 400000, 20); + pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000); + pid_set_gains(&mainboard.angle.pid, 500, 10, 4000); trajectory_d_rel(&mainboard.traj, dist); err = wait_traj_end(flags); - pid_set_gains(&mainboard.angle.pid, p, i, d); - return err; -} + if (err != END_BLOCKING) + goto fail; -/* escape from zone, and don't brake, so we can continue with another - * traj */ -uint8_t strat_escape(struct build_zone *zone, uint8_t flags) -{ - uint8_t err; - uint16_t old_spdd, old_spda; - - strat_get_speed(&old_spdd, &old_spda); - - err = WAIT_COND_OR_TIMEOUT(!opponent_is_behind(), 1000); - if (err == 0) { - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -150); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - strat_set_speed(old_spdd, old_spda); - return err; - } - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + /* we detected a blocking, reset bd, remove angle pid and + * continue */ + trajectory_d_rel(&mainboard.traj, dist); + pid_set_maximums(&mainboard.distance.pid, max_in, max_i, 4095); + pid_set_gains(&mainboard.angle.pid, 1, 0, 0); + time_wait_ms(500); + strat_hardstop(); + +#ifdef HOST_VERSION + /* issue with block on simulator */ + if (dist > 0) + trajectory_d_rel(&mainboard.traj, -10); + else + trajectory_d_rel(&mainboard.traj, 10); + wait_traj_end(END_TRAJ); +#endif - if (zone->flags & ZONE_F_DISC) { - trajectory_d_rel(&mainboard.traj, -350); - err = WAIT_COND_OR_TRAJ_END(!robot_is_near_disc(), flags); - } - else { - trajectory_d_rel(&mainboard.traj, -300); - err = wait_traj_end(flags); - } + fail: + pid_set_gains(&mainboard.angle.pid, p, i, d); + pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out); + bd_set_current_thresholds(&mainboard.distance.bd, k1, k2, i_thres, cpt_thres); - strat_set_speed(old_spdd, old_spda); - if (err == 0) - return END_NEAR; return err; } @@ -245,7 +251,7 @@ static void strat_update_traj_speed(void) a = strat_speed_a; if (strat_limit_speed_a && a > strat_limit_speed_a) a = strat_limit_speed_a; - + trajectory_set_speed(&mainboard.traj, d, a); } @@ -259,6 +265,11 @@ void strat_set_speed(uint16_t d, uint16_t a) IRQ_UNLOCK(flags); } +void strat_set_acc(double d, double a) +{ + trajectory_set_acc(&mainboard.traj, d, a); +} + void strat_get_speed(uint16_t *d, uint16_t *a) { uint8_t flags; @@ -268,6 +279,15 @@ void strat_get_speed(uint16_t *d, uint16_t *a) IRQ_UNLOCK(flags); } +void strat_get_acc(double *d, double *a) +{ + uint8_t flags; + IRQ_LOCK(flags); + *d = mainboard.traj.d_acc; + *a = mainboard.traj.a_acc; + IRQ_UNLOCK(flags); +} + void strat_limit_speed_enable(void) { strat_limit_speed_enabled = 1; @@ -278,7 +298,7 @@ void strat_limit_speed_disable(void) strat_limit_speed_enabled = 0; } -/* called periodically */ +/* called periodically (note: disabled in 2010) */ void strat_limit_speed(void) { uint16_t lim_d = 0, lim_a = 0; @@ -290,12 +310,6 @@ void strat_limit_speed(void) if (get_opponent_da(&opp_d, &opp_a) != 0) goto update; -#ifdef HOMOLOGATION - if (opp_d < 600) { - lim_d = 150; - lim_a = 200; - } -#else if (opp_d < 500) { if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) { lim_d = SPEED_DIST_VERY_SLOW; @@ -310,7 +324,6 @@ void strat_limit_speed(void) lim_a = SPEED_ANGLE_VERY_SLOW; } } -#endif else if (opp_d < 800) { if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) { lim_d = SPEED_DIST_SLOW; @@ -334,14 +347,16 @@ void strat_limit_speed(void) /* start the strat */ void strat_start(void) -{ - int8_t i; +{ uint8_t err; strat_preinit(); +#ifndef HOST_VERSION /* if start sw not plugged */ if (sensor_get(S_START_SWITCH)) { + int8_t i; + printf_P(PSTR("No start switch, press a key or plug it\r\n")); /* while start sw not plugged */ @@ -356,13 +371,14 @@ void strat_start(void) break; } } - + /* if start sw plugged */ if (!sensor_get(S_START_SWITCH)) { printf_P(PSTR("Ready, unplug start switch to start\r\n")); /* while start sw plugged */ while (!sensor_get(S_START_SWITCH)); } +#endif strat_init(); err = strat_main(); @@ -370,6 +386,7 @@ void strat_start(void) strat_exit(); } + /* return true if we have to brake due to an obstacle */ uint8_t strat_obstacle(void) { @@ -377,13 +394,22 @@ uint8_t strat_obstacle(void) int16_t opp_x, opp_y, opp_d, opp_a; /* too slow */ - if (ABS(mainboard.speed_d) < 150) + if (ABS(mainboard.speed_d) < 100) { + if (strat_obstacle_debug != 1) { + DEBUG(E_USER_STRAT, "disable opp, too slow"); + strat_obstacle_debug = 1; + } return 0; + } /* no opponent detected */ - if (get_opponent_xyda(&opp_x, &opp_y, - &opp_d, &opp_a)) + if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a) < 0) { + if (strat_obstacle_debug != 2) { + DEBUG(E_USER_STRAT, "no opponent found"); + strat_obstacle_debug = 2; + } return 0; + } /* save obstacle position */ opponent_obstacle.x = opp_x; @@ -391,34 +417,69 @@ uint8_t strat_obstacle(void) opponent_obstacle.d = opp_d; opponent_obstacle.a = opp_a; + if (!is_in_area(opp_x, opp_y, 250)) { + if (strat_obstacle_debug != 3) { + DEBUG(E_USER_STRAT, + "opponent not in area : d=%d, a=%d " + "x=%d y=%d (speed_d=%d)", + opp_d, opp_a, opp_x, opp_y, mainboard.speed_d); + strat_obstacle_debug = 3; + } + return 0; + } + /* sensor are temporarily disabled */ - if (sensor_obstacle_is_disabled()) + if (sensor_obstacle_is_disabled()) { + if (strat_obstacle_debug != 4) { + DEBUG(E_USER_STRAT, + "sensor are disabled: opponent d=%d, a=%d " + "x=%d y=%d (speed_d=%d)", + opp_d, opp_a, opp_x, opp_y, mainboard.speed_d); + strat_obstacle_debug = 4; + } return 0; + } /* relative position */ x_rel = cos(RAD(opp_a)) * (double)opp_d; y_rel = sin(RAD(opp_a)) * (double)opp_d; /* opponent too far */ - if (opp_d > 600) + if (opp_d > 650) { + if (strat_obstacle_debug != 5) { + DEBUG(E_USER_STRAT, "opponent too far d=%d, a=%d " + "x=%d y=%d (speed_d=%d)", + opp_d, opp_a, opp_x, opp_y, mainboard.speed_d); + strat_obstacle_debug = 5; + } return 0; + } /* opponent is in front of us */ - if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) { + if ((mainboard.speed_d >= 0 && opp_d < 600 && (opp_a > 315 || opp_a < 45)) || + (mainboard.speed_d >= 0 && opp_d < 800 && (opp_a > 330 || opp_a < 30))) { DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d " - "xrel=%d yrel=%d (speed_d=%d)", + "xrel=%d yrel=%d (speed_d=%d)", opp_d, opp_a, x_rel, y_rel, mainboard.speed_d); sensor_obstacle_disable(); return 1; } /* opponent is behind us */ - if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) { - DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", + if ((mainboard.speed_d < 0 && opp_d < 600 && (opp_a < 225 && opp_a > 135)) || + (mainboard.speed_d < 0 && opp_d < 800 && (opp_a < 210 && opp_a > 150))) { + DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", opp_d, opp_a, x_rel, y_rel); sensor_obstacle_disable(); return 1; } + if (strat_obstacle_debug != 6) { + DEBUG(E_USER_STRAT, "opponent not in cone d=%d, a=%d " + "x=%d y=%d (speed_d=%d)", + opp_d, opp_a, opp_x, opp_y, mainboard.speed_d); + strat_obstacle_debug = 6; + } + return 0; } @@ -433,7 +494,7 @@ void interrupt_traj_reset(void) } uint8_t test_traj_end(uint8_t why) -{ +{ uint16_t cur_timer; point_t robot_pt; @@ -451,26 +512,26 @@ uint8_t test_traj_end(uint8_t why) } if ((why & END_INTR) && traj_intr) { - interrupt_traj_reset(); + interrupt_traj_reset(); return END_INTR; } if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj)) return END_TRAJ; - + /* we are near the destination point (depends on current * speed) AND the robot is in the area bounding box. */ if (why & END_NEAR) { - int16_t d_near = 100; - + int16_t d_near = 100; + if (mainboard.speed_d > 2000) d_near = 150; - + if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) && is_in_boundingbox(&robot_pt)) return END_NEAR; } - + if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) { strat_hardstop(); return END_BLOCKING; @@ -486,6 +547,12 @@ uint8_t test_traj_end(uint8_t why) return END_OBSTACLE; } +#ifdef HOST_VERSION + if (robotsim_blocking) { + robotsim_blocking = 0; + return END_BLOCKING; + } +#endif return 0; } @@ -505,7 +572,7 @@ uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line) line, opp_x, opp_y, opp_d, opp_a); } else { - DEBUG(E_USER_STRAT, "Got %s at line %d", + DEBUG(E_USER_STRAT, "Got %s at line %d", get_err(ret), line); } return ret;