Merge branch 'maint-demo'

[eurobot/public.git] / src / robofsm / actuators.c

diff --git a/src/robofsm/actuators.c b/src/robofsm/actuators.c
index c9203a6a0938e59b45079e46cbca370fad57f17e..33ac0b1150146ebf64fbd042057eb270110ef96c 100644 (file)
--- a/src/robofsm/actuators.c
+++ b/src/robofsm/actuators.c
@@ -1,9 +1,19 @@
+/**
+ * @file actuators.c
+ * @author Martin Zidek
+ * @author Michal Sojka
+ * @author Filip Jares (?)
+ * @date 2009-2010
+ *
+ * @brief Robot's actuators control library
+ */
+
 /*
  * actuators.c                 09/02/25
  *
  * Robot's actuators control.
  *
- * Copyright: (c) 2008 CTU Dragons
+ * Copyright: (c) 2008-2010 CTU Dragons
  *            CTU FEE - Department of Control Engineering
  * License: GNU GPL v.2
  */
@@ -13,60 +23,75 @@
 
 static struct robottype_orte_data *orte = NULL;
 
-/* SERVOS */
+static uint16_t jaw_left_last_request;
+static uint16_t jaw_right_last_request;
+static uint16_t lift_last_request;
 
 void act_init(struct robottype_orte_data *ortedata)
 {
        orte = ortedata;
+       act_jaws(OPEN);
+       act_lift(0, 0, 1);
 }
 
-void act_lift2(unsigned short int position)
+void act_camera_on(void)
 {
-       orte->lift.pos = position;
-       ORTEPublicationSend(orte->publication_lift);
+       orte->camera_control.on = 1;
+       ORTEPublicationSend(orte->publication_camera_control);
 }
 
-void act_chelae(unsigned char left, unsigned char right)
+void act_camera_off(void)
 {
-       orte->chelae.left = left;
-       orte->chelae.right = right; // the hardware (chelae) responds asymetrically
-       ORTEPublicationSend(orte->publication_chelae);
+       orte->camera_control.on = 0;
+       ORTEPublicationSend(orte->publication_camera_control);
 }
 
-void act_belts(unsigned char left, unsigned char right)
+void act_lift(uint16_t req_pos, char speed, char homing)
 {
-       orte->belts.left = left;
-       orte->belts.right = right;
-       ORTEPublicationSend(orte->publication_belts);
-}
 
-void act_holder(unsigned char position)
-{
-       orte->holder.pos = position;
-       ORTEPublicationSend(orte->publication_holder);
+       orte->lift_cmd.req_pos = req_pos;
+       orte->lift_cmd.speed = speed;
+       orte->lift_cmd.homing = homing;
+       /* Remember the request so that we change check for matching
+        * response in rcv_vidle_status_cb() */
+       lift_last_request = req_pos;
+       ORTEPublicationSend(orte->publication_lift_cmd);
 }
 
-void act_pusher(unsigned short int position)
+void act_jaws(jaws_cmds cmd)
 {
-       orte->pusher.pos = position;
-       ORTEPublicationSend(orte->publication_pusher);
+        switch (cmd) {
+        case OPEN:
+                orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN;
+                usleep(300000);
+                orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN;
+                break;
+        case CLOSE:
+                orte->jaws_cmd.req_pos.left = JAW_LEFT_CLOSE;
+                usleep(300000);
+                orte->jaws_cmd.req_pos.right = JAW_RIGHT_CLOSE;
+                break;
+        case CATCH:
+                orte->jaws_cmd.req_pos.left = JAW_LEFT_CATCH;
+                orte->jaws_cmd.req_pos.right = JAW_RIGHT_CATCH;
+                break;
+        default:
+                orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN;
+                orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN;
+        }
 }
 
-void act_hokuyo(unsigned char angle)
+uint16_t act_jaw_left_get_last_reqest(void)
 {
-       orte->hokuyo_pitch.angle = angle;
-       ORTEPublicationSend(orte->publication_hokuyo_pitch);
+        return jaw_left_last_request;
 }
 
-void act_camera_on(void)
+uint16_t act_jaw_right_get_last_reqest(void)
 {
-       orte->camera_control.on = 1;
-       ORTEPublicationSend(orte->publication_camera_control);
+        return jaw_right_last_request;
 }
 
-void act_camera_off(void)
+uint16_t act_lift_get_last_reqest(void)
 {
-       orte->camera_control.on = 0;
-       ORTEPublicationSend(orte->publication_camera_control);
+       return lift_last_request;
 }
-