From 43b2c29cf87a2e8ed2cfbab531b09a5fa5ceb489 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 18 May 2012 05:59:56 +0200 Subject: digital/io-hub/src/guybrush: ignore side US sensors when upper clamp is moving --- digital/io-hub/src/guybrush/bottom_clamp.c | 6 ++++++ digital/io-hub/src/guybrush/radar_defs.c | 11 +++++++++++ digital/io-hub/src/guybrush/radar_defs.h | 3 +++ 3 files changed, 20 insertions(+) (limited to 'digital/io-hub/src/guybrush') diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c index 816e951e..2137c0f3 100644 --- a/digital/io-hub/src/guybrush/bottom_clamp.c +++ b/digital/io-hub/src/guybrush/bottom_clamp.c @@ -31,6 +31,7 @@ #include "main.h" #include "bottom_clamp.h" #include "pressure.h" +#include "radar_defs.h" #define FSM_NAME AI @@ -568,6 +569,7 @@ FSM_TRANS (CLAMP_IDLE, tree_detected,CLAMP_BOTTOM_CLAMP_HIDE_POS) FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success, CLAMP_UNFOLD_UPPER_SET) { + radar_def_upper_clamp_moving (1); IO_CLR (OUTPUT_UPPER_CLAMP_UP); IO_SET (OUTPUT_UPPER_CLAMP_DOWN); return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success); @@ -583,6 +585,7 @@ FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure, CLAMP_BLOC FSM_TRANS (CLAMP_UNFOLD_UPPER_SET, upper_set_down, CLAMP_BOTTOM_CLAMP_READY) { + radar_def_upper_clamp_moving (0); /*Putting the bottom clamp back to ready.*/ move_needed2(DECALAGE_CD_BAS,FAST_ROTATION,-1); @@ -667,6 +670,7 @@ FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_failure, CLAMP_BLO FSM_TRANS_TIMEOUT (CLAMP_RELEASE_ASSERV, TIMEOUT_FREE_ASSERV, CLAMP_FOLD_UPPER_SET) { + radar_def_upper_clamp_moving (1); IO_CLR (OUTPUT_UPPER_CLAMP_DOWN); IO_SET (OUTPUT_UPPER_CLAMP_UP); return FSM_NEXT_TIMEOUT (CLAMP_RELEASE_ASSERV); @@ -674,6 +678,7 @@ FSM_TRANS_TIMEOUT (CLAMP_RELEASE_ASSERV, TIMEOUT_FREE_ASSERV, CLAMP_FOLD_UPPER_S } FSM_TRANS (CLAMP_FOLD_UPPER_SET, upper_set_up, CLAMP_OPEN_UPPER_CLAMPS) { + radar_def_upper_clamp_moving (0); IO_SET (OUTPUT_UPPER_CLAMP_OPEN); IO_CLR (OUTPUT_UPPER_CLAMP_IN); IO_SET (OUTPUT_UPPER_CLAMP_OUT); @@ -772,6 +777,7 @@ FSM_TRANS (CLAMP_BOTTOM_CLAMP_BACK,lower_clamp_rotation_failure,CLAMP_BLOCKED) FSM_TRANS (CLAMP_UNFOLD_UPPER_SET, stop_tree_approach,CLAMP_RELEASE_ASSERV) { + radar_def_upper_clamp_moving (0); ctx.stop_tree_approach = 1; return FSM_NEXT (CLAMP_UNFOLD_UPPER_SET, stop_tree_approach); diff --git a/digital/io-hub/src/guybrush/radar_defs.c b/digital/io-hub/src/guybrush/radar_defs.c index a1228526..aefada80 100644 --- a/digital/io-hub/src/guybrush/radar_defs.c +++ b/digital/io-hub/src/guybrush/radar_defs.c @@ -40,10 +40,16 @@ struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = { { &usdist_mm[3], { -120, 0 }, G_ANGLE_UF016_DEG (180) }, }; +/** Is radar currently moving? */ +uint8_t radar_def_upper_clamp_moving_; + /** Define exclusion area (considered as invalid point). */ uint8_t radar_valid (vect_t p, uint8_t sensor) { + if (radar_def_upper_clamp_moving_ + && (sensor == RADAR_SENSOR_LEFT || sensor == RADAR_SENSOR_RIGHT)) + return 0; return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM && (sensor == RADAR_SENSOR_FRONT || sensor == RADAR_SENSOR_BACK @@ -55,3 +61,8 @@ radar_valid (vect_t p, uint8_t sensor) && p.x < PG_TOTEM_RIGHT_X + PG_TOTEM_WIDTH_MM / 2 + RADAR_MARGIN_MM)))); } +void +radar_def_upper_clamp_moving (uint8_t moving) +{ + radar_def_upper_clamp_moving_ = moving; +} diff --git a/digital/io-hub/src/guybrush/radar_defs.h b/digital/io-hub/src/guybrush/radar_defs.h index 83f74407..8bb683ee 100644 --- a/digital/io-hub/src/guybrush/radar_defs.h +++ b/digital/io-hub/src/guybrush/radar_defs.h @@ -38,4 +38,7 @@ #define RADAR_SENSOR_BACK_FIRST 3 #define RADAR_SENSOR_BACK_NB 1 +void +radar_def_upper_clamp_moving (uint8_t moving); + #endif /* radar_defs_h */ -- cgit v1.2.3