X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;f=projects%2Fmicrob2010%2Fballboard%2Factuator.c;h=cf9517fa963160e476bc380a1bf1e03c0fb95b7e;hb=8d1a172c863bc0712e8d92f3e808e85a4e579e10;hp=7365427cd61d5625b1dfff9b0ec617046370edd2;hpb=6914527de2ecfef9d790740c71739e7418246b96;p=aversive.git

diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c
index 7365427..cf9517f 100644
--- a/projects/microb2010/ballboard/actuator.c
+++ b/projects/microb2010/ballboard/actuator.c
@@ -40,18 +40,18 @@
 
 #include <rdline.h>
 
+#include "actuator.h"
 #include "main.h"
 
-#define ROLLER_ON      -1200
+#define ROLLER_ON      -ROLLER_SPEED
 #define ROLLER_OFF     0
-#define ROLLER_REVERSE 1200
+#define ROLLER_REVERSE ROLLER_SPEED
 
-#define FORKROT_DEPLOYED -50000
-#define FORKROT_PACKED   0
-
-#define FORKTRANS_LEFT   0
-#define FORKTRANS_MIDDLE 500
-#define FORKTRANS_RIGHT  1000
+#define FORKROT_DEPLOYED -55000
+#define FORKROT_MID1 -31000
+#define FORKROT_MID2 -27000
+#define FORKROT_EJECT   -12000
+#define FORKROT_PACKED   -4000
 
 void roller_on(void)
 {
@@ -78,17 +78,44 @@ void fork_pack(void)
 	cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED);
 }
 
-void fork_left(void)
+void fork_mid1(void)
+{
+	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)
 {
-	cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_LEFT);
+	int32_t diff;
+	diff = pos - encoders_spi_get_value(FORKROT_ENCODER);
+	if (diff < 0)
+		diff = -diff;
+	if (diff < 500)
+		return 1;
+	return 0;
 }
 
-void fork_right(void)
+uint8_t fork_is_packed(void)
 {
-	cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_RIGHT);
+	return fork_is_at_pos(FORKROT_PACKED);
 }
 
-void fork_middle(void)
+void actuator_init(void)
 {
-	cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_MIDDLE);
+	printf_P(PSTR("fork autopos..."));
+	pwm_ng_set(FORKROT_PWM, 400);
+	wait_ms(1000);
+	pwm_ng_set(FORKROT_PWM, 0);
+	encoders_spi_set_value(FORKROT_ENCODER, 0);
+	printf_P(PSTR("ok\r\n"));
+	ballboard.forkrot.on = 1;
 }