summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-13 02:59:20 +0200
committerNicolas Schodet2010-05-13 02:59:20 +0200
commit44f55257cae560ee1e949d378504a9a457eb71c0 (patch)
tree74066f7ed9b3a6f57405e8db701994ed3d2b6fb8
parent645b7d11df890ed99e5b00c8272d4e794060f703 (diff)
digital/asserv, digital/io, host/simu: change gate contact position
-rw-r--r--digital/asserv/src/asserv/simu.host.c2
-rw-r--r--digital/io/src/ai_loader_cb.c18
-rw-r--r--digital/io/src/ai_top_cb.c2
-rw-r--r--digital/io/src/bot.h2
-rw-r--r--digital/io/src/loader.fsm8
-rw-r--r--digital/io/tools/test_simu_control.py2
-rw-r--r--host/simu/robots/marcel/model/loader.py2
-rw-r--r--host/simu/robots/marcel/view/loader.py4
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);
}
@@ -140,16 +138,6 @@ ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void)
}
/*
- * 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
* close gate
@@ -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)