From 44f55257cae560ee1e949d378504a9a457eb71c0 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 02:59:20 +0200 Subject: digital/asserv, digital/io, host/simu: change gate contact position --- digital/asserv/src/asserv/simu.host.c | 2 +- digital/io/src/ai_loader_cb.c | 18 +++--------------- digital/io/src/ai_top_cb.c | 2 +- digital/io/src/bot.h | 2 +- digital/io/src/loader.fsm | 8 +------- digital/io/tools/test_simu_control.py | 2 +- host/simu/robots/marcel/model/loader.py | 2 +- host/simu/robots/marcel/view/loader.py | 4 ++-- 8 files changed, 11 insertions(+), 29 deletions(-) diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 69b0c2f4..019f8864 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -267,7 +267,7 @@ simu_sensor_update_marcel (void) PINC |= sensors_bit[i]; } /** Top zero sensors. */ - if (simu_aux_model[1].th > 0) + if (simu_aux_model[1].th < 0) PINC |= IO_BV (CONTACT_AUX1_ZERO_IO); } diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index 7de8a652..a51d740e 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -63,7 +63,7 @@ ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void) asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED); mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); - asserv_motor1_zero_position (BOT_GATE_SPEED); + asserv_motor1_zero_position (-BOT_GATE_SPEED); return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot); } @@ -115,13 +115,11 @@ ai__LOADER_INIT_GATE_ZERO__gate_succeed (void) /* * LOADER_INIT_ELEVATOR_UP =elevator_succeed=> - * => LOADER_INIT_GATE_OPEN - * open gate + * => LOADER_INIT_GATE_WAIT */ fsm_branch_t ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void) { - asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_succeed); } @@ -139,16 +137,6 @@ ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void) return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed); } -/* - * LOADER_INIT_GATE_OPEN =gate_succeed=> - * => LOADER_INIT_GATE_WAIT - */ -fsm_branch_t -ai__LOADER_INIT_GATE_OPEN__gate_succeed (void) -{ - return ai_next (LOADER_INIT_GATE_OPEN, gate_succeed); -} - /* * LOADER_INIT_GATE_WAIT =state_timeout=> * => LOADER_INIT_GATE_CLOSE @@ -157,7 +145,7 @@ ai__LOADER_INIT_GATE_OPEN__gate_succeed (void) fsm_branch_t ai__LOADER_INIT_GATE_WAIT__state_timeout (void) { - asserv_move_motor1_absolute (0, BOT_GATE_SPEED); + asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); return ai_next (LOADER_INIT_GATE_WAIT, state_timeout); } diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index c057bebf..6e0a0c8d 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -189,7 +189,7 @@ ai__UNLOAD_FACE_BIN__bot_move_succeed (void) fsm_branch_t ai__UNLOAD_BACK_BIN__bot_move_succeed (void) { - asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); + asserv_move_motor1_absolute (0, BOT_GATE_SPEED); return ai_next (UNLOAD_BACK_BIN, bot_move_succeed); } diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h index 9617ecb2..582f3261 100644 --- a/digital/io/src/bot.h +++ b/digital/io/src/bot.h @@ -116,7 +116,7 @@ #define BOT_GATE_SPEED 0x46 /** Gate stroke in steps. */ -#define BOT_GATE_STROKE_STEP 0x2081 +#define BOT_GATE_STROKE_STEP -0x1d6b /** * Definition of the colors. diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 1978ef41..dd0a6a9c 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -19,8 +19,6 @@ States: find gate zero LOADER_INIT_ELEVATOR_UP move the elevator up so that the robot can go to the wall - LOADER_INIT_GATE_OPEN - open gate to unload elements before start LOADER_INIT_GATE_WAIT[timeout=225] wait with gate open LOADER_INIT_GATE_CLOSE @@ -99,14 +97,10 @@ LOADER_INIT_GATE_ZERO: gate_succeed -> LOADER_INIT_ELEVATOR_UP LOADER_INIT_ELEVATOR_UP: - elevator_succeed -> LOADER_INIT_GATE_OPEN - open gate + elevator_succeed -> LOADER_INIT_GATE_WAIT elevator_failed -> LOADER_IDLE initialisation failure -LOADER_INIT_GATE_OPEN: - gate_succeed -> LOADER_INIT_GATE_WAIT - LOADER_INIT_GATE_WAIT: state_timeout -> LOADER_INIT_GATE_CLOSE close gate diff --git a/digital/io/tools/test_simu_control.py b/digital/io/tools/test_simu_control.py index 1f51f1c2..9365ab2e 100644 --- a/digital/io/tools/test_simu_control.py +++ b/digital/io/tools/test_simu_control.py @@ -90,7 +90,7 @@ class TestSimuControl (TestSimu): def gate_command (self): if self.gate_var.get (): - pos = 0x2081 + pos = -0x1d6b else: pos = 0 self.asserv.goto_pos ('a1', pos) diff --git a/host/simu/robots/marcel/model/loader.py b/host/simu/robots/marcel/model/loader.py index 79292ecd..0270ce79 100644 --- a/host/simu/robots/marcel/model/loader.py +++ b/host/simu/robots/marcel/model/loader.py @@ -36,7 +36,7 @@ class Loader (Observable): ELEVATOR_LINEAR_STROKE = 32 ELEVATOR_ROTATING_STROKE = 200 - 32 - GATE_STROKE = 3.3284 * pi + GATE_STROKE = -3.0124 * 2 * pi FRONT_ZONE_X_MIN = 120 - 15 FRONT_ZONE_X_MAX = 120 + CLAMP_LENGTH - 15 diff --git a/host/simu/robots/marcel/view/loader.py b/host/simu/robots/marcel/view/loader.py index 4ad2ffce..5bda5d5b 100644 --- a/host/simu/robots/marcel/view/loader.py +++ b/host/simu/robots/marcel/view/loader.py @@ -60,11 +60,11 @@ class Loader (Drawable): x = 0 y += 40 if self.model.gate_angle is not None: - ratio = self.model.gate_angle / self.model.GATE_STROKE + ratio = 1 - self.model.gate_angle / self.model.GATE_STROKE # Draw gate. for i in xrange (0, 4): self.draw_line ((100, 125 + 5 * i), - (300, 25 + 25 * i + (75 - 10 * i) * ratio)) + (300, 25 + 25 * i + (150 - 20 * i) * ratio)) if self.model.elevator_height is not None: self.trans_identity () self.trans_rotate (-self.model.elevator_angle) -- cgit v1.2.3