robofsm: fix servos stuffs.

diff --git a/src/robofsm/eb2008/servos_eb2008.c b/src/robofsm/eb2008/servos_eb2008.c
index c527ca4239a9a20eb2d7fc7768398dbdbe400ef1..cc0cd1f7b66381eef2abbd35f18364d69e3c1bbb 100644 (file)
--- a/src/robofsm/eb2008/servos_eb2008.c
+++ b/src/robofsm/eb2008/servos_eb2008.c
@@ -11,6 +11,7 @@
 #include <robot_eb2008.h>
 #include <servos_eb2008.h>
 
+/* SERVOS */
 void open_top_door()
 {
        robot.orte.servos.door_top = TOP_DOOR_OPEN;
@@ -61,6 +62,19 @@ void close_brush_right()
        robot.orte.servos.brush_right = BRUSH_RIGHT_CLOSE;
 }
 
+void brushes_in()
+{
+       close_brush_left();
+       close_brush_right();
+}
+
+void brushes_out()
+{
+       open_brush_left();
+       open_brush_right();
+}
+
+/* DRIVES */
 void on_brush_left()
 {
        robot.orte.drives.brush_left = LEFT_BRUSH_DRIVE_ON;
@@ -81,16 +95,16 @@ void off_brush_right()
        robot.orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
 }
 
-void brushes_in()
+void brushes_drives_in()
 {
-       close_brush_left();
-       close_brush_right();
+       robot.orte.drives.brush_left = LEFT_BRUSH_DRIVE_IN;
+       robot.orte.drives.brush_right = RIGHT_BRUSH_DRIVE_IN;
 }
 
-void brushes_out()
+void brushes_drive_sout()
 {
-       open_brush_left();
-       open_brush_right();
+       robot.orte.drives.brush_left = LEFT_BRUSH_DRIVE_OUT;
+       robot.orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OUT;
 }
 
 void on_bagr()
@@ -113,13 +127,18 @@ void set_carousel(unsigned char position)
        robot.orte.drives.carousel_pos = position;
 }
 
-void on_laser_engine()
+void on_laser()
+{
+       robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
+}
+
+void off_laser()
 {
-       robot.orte.laser_cmd.speed = LASER_ENGINE_ON;
+       robot.orte.laser_cmd.speed = LASER_DRIVE_OFF;
 }
 
-void off_laser_engine()
+void set_laser(unsigned char speed)
 {
-       robot.orte.laser_cmd.speed = LASER_ENGINE_OFF;
+       robot.orte.laser_cmd.speed = speed % 4;
 }
 

diff --git a/src/robofsm/eb2008/servos_eb2008.h b/src/robofsm/eb2008/servos_eb2008.h
index f59d070b52c1517dc6c6c73e92fd8c5c4e85aab6..2aad6d81e595b14aa4017ec9102acdba5b53ce2c 100644 (file)
--- a/src/robofsm/eb2008/servos_eb2008.h
+++ b/src/robofsm/eb2008/servos_eb2008.h
@@ -11,77 +11,35 @@
 #ifndef SERVOS_EB2008_H
 #define SERVOS_EB2008_H
 
-#ifndef LEFT_BRUSH_DRIVE_ON
-#define LEFT_BRUSH_DRIVE_ON 150
-#endif
-
-#ifndef LEFT_BRUSH_DRIVE_OFF
-#define LEFT_BRUSH_DRIVE_OFF 100
-#endif
-
-#ifndef RIGHT_BRUSH_DRIVE_ON
-#define RIGHT_BRUSH_DRIVE_ON 150
-#endif
-
-#ifndef RIGHT_BRUSH_DRIVE_OFF
-#define RIGHT_BRUSH_DRIVE_OFF 100
-#endif
-
-#ifndef BAGR_DRIVE_ON
-#define BAGR_DRIVE_ON 150
-#endif
-
-#ifndef BAGR_DRIVE_OFF
-#define BAGR_DRIVE_OFF 100
-#endif
-
-#ifndef TOP_DOOR_OPEN
+/* SERVOS */
 #define TOP_DOOR_OPEN 127
-#endif
-
-#ifndef TOP_DOOR_CLOSE
 #define TOP_DOOR_CLOSE 127
-#endif
-
-#ifndef BOTTOM_DOOR_OPEN
 #define BOTTOM_DOOR_OPEN 5
-#endif
-
-#ifndef BOTTOM_DOOR_CLOSE
 #define BOTTOM_DOOR_CLOSE 208
-#endif
-
-#ifndef BACK_DOOR_OPEN
 #define BACK_DOOR_OPEN 0
-#endif
-
-#ifndef BACK_DOOR_CLOSE
 #define BACK_DOOR_CLOSE 255
-#endif
 
-#ifndef BRUSH_LEFT_CLOSE
 #define BRUSH_LEFT_CLOSE 150
-#endif
-
-#ifndef BRUSH_RIGHT_CLOSE
 #define BRUSH_RIGHT_CLOSE 150
-#endif
-
-#ifndef BRUSH_LEFT_OPEN
 #define BRUSH_LEFT_OPEN 0
-#endif
-
-#ifndef BRUSH_RIGHT_OPEN
 #define BRUSH_RIGHT_OPEN 0
-#endif
 
-#ifndef LASER_ENGINE_ON
-#define LASER_ENGINE_ON 1
-#endif
+/* DRIVES */
+#define LEFT_BRUSH_DRIVE_ON 150
+#define LEFT_BRUSH_DRIVE_OFF 100
+#define RIGHT_BRUSH_DRIVE_ON 150
+#define RIGHT_BRUSH_DRIVE_OFF 100
+
+#define LEFT_BRUSH_DRIVE_IN 150
+#define RIGHT_BRUSH_DRIVE_IN 50
+#define LEFT_BRUSH_DRIVE_OUT 50
+#define RIGHT_BRUSH_DRIVE_OUT 150
+
+#define BAGR_DRIVE_ON 150
+#define BAGR_DRIVE_OFF 100
 
-#ifndef LASER_ENGINE_OFF
-#define LASER_ENGINE_OFF 0
-#endif
+#define LASER_DRIVE_ON 1
+#define LASER_DRIVE_OFF 0
 
 #ifdef __cplusplus
 extern "C" {
@@ -102,12 +60,15 @@ void on_brush_right();
 void off_brush_right();
 void brushes_in();
 void brushes_out();
+void brushes_drives_in();
+void brushes_drives_out();
 void on_bagr();
 void off_bagr();
 void set_bagr(unsigned char speed);
 void set_carousel(unsigned char position);
-void on_laser_engine();
-void off_laser_engine();
+void on_laser();
+void off_laser();
+void set_laser(unsigned char speed);
 #ifdef __cplusplus
 }
 #endif