robofsm: add function open, close, catch for jaws

diff --git a/src/robofsm/actuators.c b/src/robofsm/actuators.c
index cddd434d7aead9d1d385142756af952910baf2bc..607930dd2c96388cdecaaa80144f6da87e9d8573 100644 (file)
--- a/src/robofsm/actuators.c
+++ b/src/robofsm/actuators.c
@@ -54,9 +54,25 @@ void act_lift(uint16_t position, char speed)
        ORTEPublicationSend(orte->publication_lift_cmd);
 }
 
-void act_jaws(struct robottype_orte_data *orte_data)
-{
-        //orte->jaws_cmd;
+void act_jaws(struct robottype_orte_data *orte_data, jaws_cmds cmd)
+{       
+  switch (cmd) {
+      case OPEN:
+        orte->jaws_cmd.req_pos.left=0x1C3;
+        orte->jaws_cmd.req_pos.right=0x1B3;
+        break;
+      case CLOSE:
+          orte->jaws_cmd.req_pos.left=0x30F;
+          orte->jaws_cmd.req_pos.right=0x36F;
+          break;
+      case CATCH:
+          orte->jaws_cmd.req_pos.left=0x1C3;
+          orte->jaws_cmd.req_pos.right=0x1B3;
+          break;
+      default:
+          orte->jaws_cmd.req_pos.left=0x1C3;
+          orte->jaws_cmd.req_pos.right=0x1B3;
+      }
 }
 
 uint16_t act_jaw_left_get_last_reqest(void)

diff --git a/src/robofsm/actuators.h b/src/robofsm/actuators.h
index 1d4d66350881fe520aa00e258a7ad04df9878891..4b8bf219a6087efe36b645fec74442fcc3bba618 100644 (file)
--- a/src/robofsm/actuators.h
+++ b/src/robofsm/actuators.h
@@ -31,16 +31,14 @@ of the robot.
 #define HOKUYO_PITCH_HORIZONTAL 0xC0
 #define HOKUYO_PITCH_MIN       0x00
 
-#define VIDLE_UP               0x3c0
-#define VIDLE_MIDDLE           0x250
-#define VIDLE_LOAD_PREPARE     0x178
-#define VIDLE_DOWN             0x170
-
-#define VIDLE_FAST_SPEED       0x00
-#define VIDLE_MEDIUM_SPEED     0x0a
-
 #include <stdint.h>
 
+typedef enum {  
+  OPEN,
+  CLOSE,
+  CATCH
+} jaws_cmds;
+
 #ifdef __cplusplus
 extern "C" {
 #endif 
@@ -51,7 +49,7 @@ void act_camera_on(void);
 void act_camera_off(void);
 
 void act_lift(uint16_t position, char speed);
-void act_jaws(struct robottype_orte_data *orte_data);
+void act_jaws(robottype_orte_data* orte_data, jaws_cmds cmd);
 
 uint16_t act_jaw_left_get_last_reqest(void);
 uint16_t act_jaw_right_get_last_reqest(void);