X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fballboard%2Factuator.c;fp=projects%2Fmicrob2010%2Fballboard%2Factuator.c;h=cf9517fa963160e476bc380a1bf1e03c0fb95b7e;hp=c635fe92f3ea6f3eac2a797dad22aa20b097e6c7;hb=04f1061f2c5959ce87f632591576097f540dfc14;hpb=e6e4582229cd4ed8ffe73bdfbae79d236129df3b

diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c
index c635fe9..cf9517f 100644
--- a/projects/microb2010/ballboard/actuator.c
+++ b/projects/microb2010/ballboard/actuator.c
@@ -48,7 +48,9 @@
 #define ROLLER_REVERSE ROLLER_SPEED
 
 #define FORKROT_DEPLOYED -55000
-#define FORKROT_MID -33000
+#define FORKROT_MID1 -31000
+#define FORKROT_MID2 -27000
+#define FORKROT_EJECT   -12000
 #define FORKROT_PACKED   -4000
 
 void roller_on(void)
@@ -76,9 +78,35 @@ void fork_pack(void)
 	cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED);
 }
 
-void fork_mid(void)
+void fork_mid1(void)
 {
-	cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID);
+	cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID1);
+}
+
+void fork_mid2(void)
+{
+	cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID2);
+}
+
+void fork_eject(void)
+{
+	cs_set_consign(&ballboard.forkrot.cs, FORKROT_EJECT);
+}
+
+static uint8_t fork_is_at_pos(int32_t pos)
+{
+	int32_t diff;
+	diff = pos - encoders_spi_get_value(FORKROT_ENCODER);
+	if (diff < 0)
+		diff = -diff;
+	if (diff < 500)
+		return 1;
+	return 0;
+}
+
+uint8_t fork_is_packed(void)
+{
+	return fork_is_at_pos(FORKROT_PACKED);
 }
 
 void actuator_init(void)