french cup + idf cup

[aversive.git] / projects / microb2010 / ballboard / actuator.c

diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c
index 367e241..cf9517f 100644 (file)
--- a/projects/microb2010/ballboard/actuator.c
+++ b/projects/microb2010/ballboard/actuator.c
@@ -40,5 +40,82 @@
 
 #include <rdline.h>
 
+#include "actuator.h"
 #include "main.h"
 
+#define ROLLER_ON      -ROLLER_SPEED
+#define ROLLER_OFF     0
+#define ROLLER_REVERSE ROLLER_SPEED
+
+#define FORKROT_DEPLOYED -55000
+#define FORKROT_MID1 -31000
+#define FORKROT_MID2 -27000
+#define FORKROT_EJECT   -12000
+#define FORKROT_PACKED   -4000
+
+void roller_on(void)
+{
+       cs_set_consign(&ballboard.roller.cs, ROLLER_ON);
+}
+
+void roller_off(void)
+{
+       cs_set_consign(&ballboard.roller.cs, ROLLER_OFF);
+}
+
+void roller_reverse(void)
+{
+       cs_set_consign(&ballboard.roller.cs, ROLLER_REVERSE);
+}
+
+void fork_deploy(void)
+{
+       cs_set_consign(&ballboard.forkrot.cs, FORKROT_DEPLOYED);
+}
+
+void fork_pack(void)
+{
+       cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED);
+}
+
+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)
+{
+       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)
+{
+       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;
+}