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 }
  34
  35 void act_camera_on(void)
  36 {
  37         orte->camera_control.on = 1;
  38         ORTEPublicationSend(orte->publication_camera_control);
  39 }
  40
  41 void act_camera_off(void)
  42 {
  43         orte->camera_control.on = 0;
  44         ORTEPublicationSend(orte->publication_camera_control);
  45 }
  46
  47 void act_lift(uint16_t position, char speed)
  48 {
  49         orte->lift_cmd.req_pos = position;
  50         orte->lift_cmd.speed = speed;
  51         /* Remember the request so that we change check for matching
  52          * response in rcv_vidle_status_cb() */
  53         lift_last_request = position;
  54         ORTEPublicationSend(orte->publication_lift_cmd);
  55 }
  56
  57 void act_jaws(jaws_cmds cmd)
  58 {
  59   switch (cmd) {
  60       case OPEN:
  61         orte->jaws_cmd.req_pos.left=0x1C3;
  62         orte->jaws_cmd.req_pos.right=0x1B3;
  63         break;
  64       case CLOSE:
  65           orte->jaws_cmd.req_pos.left=0x30F;
  66           orte->jaws_cmd.req_pos.right=0x36F;
  67           break;
  68       case CATCH:
  69           orte->jaws_cmd.req_pos.left = 0x25F;
  70           orte->jaws_cmd.req_pos.right = 0x27F;
  71           break;
  72       default:
  73           orte->jaws_cmd.req_pos.left=0x1C3;
  74           orte->jaws_cmd.req_pos.right=0x1B3;
  75       }
  76 }
  77
  78 uint16_t act_jaw_left_get_last_reqest(void)
  79 {
  80         return jaw_left_last_request;
  81 }
  82
  83 uint16_t act_jaw_right_get_last_reqest(void)
  84 {
  85         return jaw_right_last_request;
  86 }
  87
  88 uint16_t act_lift_get_last_reqest(void)
  89 {
  90         return lift_last_request;
  91 }