summaryrefslogtreecommitdiff
path: root/digital/io-hub
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c6
-rw-r--r--digital/io-hub/src/guybrush/radar_defs.c11
-rw-r--r--digital/io-hub/src/guybrush/radar_defs.h3
3 files changed, 20 insertions, 0 deletions
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 */