eb_jaws_11: update jaws min, max position

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

diff --git a/src/robofsm/actuators.c b/src/robofsm/actuators.c
index b1a079198a0d53ea6c4c37209bafebb02f3edb90..4ca6d16677740e05e9356dd471948be5e2ce2998 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,42 +23,69 @@
 
 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;
 }
 
-void act_lift(unsigned position)
+void act_camera_on(void)
+{
+       orte->camera_control.on = 1;
+       ORTEPublicationSend(orte->publication_camera_control);
+}
+
+void act_camera_off(void)
 {
-       orte->lift.pos = position;
-       ORTEPublicationSend(orte->publication_lift);
+       orte->camera_control.on = 0;
+       ORTEPublicationSend(orte->publication_camera_control);
 }
 
-void act_chelas(unsigned left, unsigned right)
+void act_lift(uint16_t position, char speed)
 {
-       orte->chelas.left = left;
-       orte->chelas.right = right;
-       ORTEPublicationSend(orte->publication_chelas);
+       orte->lift_cmd.req_pos = position;
+       orte->lift_cmd.speed = speed;
+       /* Remember the request so that we change check for matching
+        * response in rcv_vidle_status_cb() */
+       lift_last_request = position;
+       ORTEPublicationSend(orte->publication_lift_cmd);
 }
 
-void act_belts(int left, int right)
+void act_jaws(jaws_cmds cmd)
 {
-       orte->belts.left = left;
-       orte->belts.right = right;
-       ORTEPublicationSend(orte->publication_belts);
+  switch (cmd) {
+      case OPEN:
+         orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN;
+         orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN;
+         break;
+      case CLOSE:
+          orte->jaws_cmd.req_pos.left = JAW_LEFT_CLOSE;
+          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_holder(unsigned position)
+uint16_t act_jaw_left_get_last_reqest(void)
 {
-       orte->holder.pos = position;
-       ORTEPublicationSend(orte->publication_holder);
+        return jaw_left_last_request;
 }
 
-void act_pusher(unsigned position)
+uint16_t act_jaw_right_get_last_reqest(void)
 {
-       orte->pusher.pos = position;
-       ORTEPublicationSend(orte->publication_pusher);
+        return jaw_right_last_request;
 }
 
+uint16_t act_lift_get_last_reqest(void)
+{
+       return lift_last_request;
+}