src/robofsm/actuators.c

   1 /**
   2  * @file actuators.c
   3  * @author Martin Zidek
   4  * @author Michal Sojka
   5  * @author Filip Jares (?)
   6  * @date 2009-2010
   7  *
   8  * @brief Robot's actuators control library
   9  */
  10
  11 /*
  12  * actuators.c                  09/02/25
  13  *
  14  * Robot's actuators control.
  15  *
  16  * Copyright: (c) 2008-2010 CTU Dragons
  17  *            CTU FEE - Department of Control Engineering
  18  * License: GNU GPL v.2
  19  */
  20
  21 #include <robot.h>
  22 #include <actuators.h>
  23
  24 static struct robottype_orte_data *orte = NULL;
  25
  26 static uint16_t jaw_left_last_request;
  27 static uint16_t jaw_right_last_request;
  28 static uint16_t lift_last_request;
  29
  30 void act_init(struct robottype_orte_data *ortedata)
  31 {
  32         orte = ortedata;
  33         act_jaws(OPEN);
  34         act_lift(0, 0, 1);
  35 }
  36
  37 void act_camera_on(void)
  38 {
  39         orte->camera_control.on = 1;
  40         ORTEPublicationSend(orte->publication_camera_control);
  41 }
  42
  43 void act_camera_off(void)
  44 {
  45         orte->camera_control.on = 0;
  46         ORTEPublicationSend(orte->publication_camera_control);
  47 }
  48
  49 void act_lift(uint16_t req_pos, char speed, char homing)
  50 {
  51
  52         orte->lift_cmd.req_pos = req_pos;
  53         orte->lift_cmd.speed = speed;
  54         orte->lift_cmd.homing = homing;
  55         /* Remember the request so that we change check for matching
  56          * response in rcv_vidle_status_cb() */
  57         lift_last_request = req_pos;
  58         ORTEPublicationSend(orte->publication_lift_cmd);
  59 }
  60
  61 void act_jaws(jaws_cmds cmd)
  62 {
  63         switch (cmd) {
  64         case OPEN:
  65                 orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN;
  66                 usleep(300000);
  67                 orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN;
  68                 break;
  69         case CLOSE:
  70                 orte->jaws_cmd.req_pos.left = JAW_LEFT_CLOSE;
  71                 usleep(300000);
  72                 orte->jaws_cmd.req_pos.right = JAW_RIGHT_CLOSE;
  73                 break;
  74         case CATCH:
  75                 orte->jaws_cmd.req_pos.left = JAW_LEFT_CATCH;
  76                 orte->jaws_cmd.req_pos.right = JAW_RIGHT_CATCH;
  77                 break;
  78         default:
  79                 orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN;
  80                 orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN;
  81         }
  82 }
  83
  84 uint16_t act_jaw_left_get_last_reqest(void)
  85 {
  86         return jaw_left_last_request;
  87 }
  88
  89 uint16_t act_jaw_right_get_last_reqest(void)
  90 {
  91         return jaw_right_last_request;
  92 }
  93
  94 uint16_t act_lift_get_last_reqest(void)
  95 {
  96         return lift_last_request;
  97 }