robot arm demo team / Mbed 2 deprecated RobotArmDemo Featured

demo project

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

RobotNode/NodeAX12.cpp@15:4bd10f531cdc, 2016-01-19 (annotated)

Committer:
henryrawas
Date:
Tue Jan 19 19:50:07 2016 +0000
Revision:
15:4bd10f531cdc
Parent:
13:ffeff9b5e513
Child:
19:2f0ec9ac1238
read all values at once and cache. Other cleanups

Who changed what in which revision?

UserRevisionLine numberNew contents of line
henryrawas 12:ac6c9d7f8c40 1
henryrawas 12:ac6c9d7f8c40 2 #include "mbed.h"
henryrawas 12:ac6c9d7f8c40 3 #include "NodeAX12.h"
henryrawas 12:ac6c9d7f8c40 4
henryrawas 12:ac6c9d7f8c40 5
henryrawas 12:ac6c9d7f8c40 6 NodeAX12::NodeAX12(DynamixelBus* pbus, ServoId ID) :
henryrawas 12:ac6c9d7f8c40 7 _Servo(pbus, ID)
henryrawas 12:ac6c9d7f8c40 8 {
henryrawas 15:4bd10f531cdc 9
henryrawas 12:ac6c9d7f8c40 10 }
henryrawas 12:ac6c9d7f8c40 11
henryrawas 12:ac6c9d7f8c40 12
henryrawas 12:ac6c9d7f8c40 13 bool NodeAX12::HasMeasure(int measureId)
henryrawas 12:ac6c9d7f8c40 14 {
henryrawas 12:ac6c9d7f8c40 15 switch (measureId)
henryrawas 12:ac6c9d7f8c40 16 {
henryrawas 12:ac6c9d7f8c40 17 case NM_Temperature:
henryrawas 12:ac6c9d7f8c40 18 return true;
henryrawas 12:ac6c9d7f8c40 19
henryrawas 12:ac6c9d7f8c40 20 case NM_Degrees:
henryrawas 12:ac6c9d7f8c40 21 return true;
henryrawas 12:ac6c9d7f8c40 22
henryrawas 12:ac6c9d7f8c40 23 case NM_Voltage:
henryrawas 12:ac6c9d7f8c40 24 return true;
henryrawas 12:ac6c9d7f8c40 25
henryrawas 12:ac6c9d7f8c40 26 case NM_Load:
henryrawas 12:ac6c9d7f8c40 27 return true;
henryrawas 12:ac6c9d7f8c40 28
henryrawas 12:ac6c9d7f8c40 29 default:
henryrawas 12:ac6c9d7f8c40 30 return false;
henryrawas 12:ac6c9d7f8c40 31 }
henryrawas 12:ac6c9d7f8c40 32 }
henryrawas 12:ac6c9d7f8c40 33
henryrawas 12:ac6c9d7f8c40 34 float NodeAX12::GetMeasure(int measureId)
henryrawas 12:ac6c9d7f8c40 35 {
henryrawas 12:ac6c9d7f8c40 36 switch (measureId)
henryrawas 12:ac6c9d7f8c40 37 {
henryrawas 12:ac6c9d7f8c40 38 case NM_Temperature:
henryrawas 15:4bd10f531cdc 39 return (float)_Servo.GetTemperature();
henryrawas 12:ac6c9d7f8c40 40
henryrawas 12:ac6c9d7f8c40 41 case NM_Degrees:
henryrawas 15:4bd10f531cdc 42 return (float)_Servo.GetPosition();
henryrawas 12:ac6c9d7f8c40 43
henryrawas 12:ac6c9d7f8c40 44 case NM_Voltage:
henryrawas 15:4bd10f531cdc 45 return (float)_Servo.GetSupplyVoltage();
henryrawas 12:ac6c9d7f8c40 46
henryrawas 12:ac6c9d7f8c40 47 case NM_Load:
henryrawas 15:4bd10f531cdc 48 return (float)_Servo.GetLoad();
henryrawas 12:ac6c9d7f8c40 49
henryrawas 12:ac6c9d7f8c40 50 default:
henryrawas 12:ac6c9d7f8c40 51 return 0.0f;
henryrawas 12:ac6c9d7f8c40 52 }
henryrawas 12:ac6c9d7f8c40 53 }
henryrawas 12:ac6c9d7f8c40 54
henryrawas 15:4bd10f531cdc 55 void NodeAX12::ClearMeasureCache()
henryrawas 12:ac6c9d7f8c40 56 {
henryrawas 15:4bd10f531cdc 57 _Servo.ClearCache();
henryrawas 12:ac6c9d7f8c40 58 }
henryrawas 12:ac6c9d7f8c40 59
henryrawas 12:ac6c9d7f8c40 60 bool NodeAX12::HasAction(int actionId)
henryrawas 12:ac6c9d7f8c40 61 {
henryrawas 12:ac6c9d7f8c40 62 switch (actionId)
henryrawas 12:ac6c9d7f8c40 63 {
henryrawas 12:ac6c9d7f8c40 64 case NA_Ping:
henryrawas 12:ac6c9d7f8c40 65 return true;
henryrawas 12:ac6c9d7f8c40 66
henryrawas 12:ac6c9d7f8c40 67 case NA_Init:
henryrawas 12:ac6c9d7f8c40 68 return true;
henryrawas 12:ac6c9d7f8c40 69
henryrawas 12:ac6c9d7f8c40 70 case NA_Rotate:
henryrawas 12:ac6c9d7f8c40 71 return true;
henryrawas 12:ac6c9d7f8c40 72
henryrawas 12:ac6c9d7f8c40 73 case NA_ClearError:
henryrawas 12:ac6c9d7f8c40 74 return true;
henryrawas 12:ac6c9d7f8c40 75
henryrawas 12:ac6c9d7f8c40 76 default:
henryrawas 12:ac6c9d7f8c40 77 return false;
henryrawas 12:ac6c9d7f8c40 78 }
henryrawas 12:ac6c9d7f8c40 79 }
henryrawas 12:ac6c9d7f8c40 80
henryrawas 12:ac6c9d7f8c40 81
henryrawas 12:ac6c9d7f8c40 82 bool NodeAX12::DoAction(int actionId, float actionValue)
henryrawas 12:ac6c9d7f8c40 83 {
henryrawas 12:ac6c9d7f8c40 84 StatusCode sc;
henryrawas 12:ac6c9d7f8c40 85
henryrawas 12:ac6c9d7f8c40 86 switch (actionId)
henryrawas 12:ac6c9d7f8c40 87 {
henryrawas 12:ac6c9d7f8c40 88 case NA_Ping:
henryrawas 12:ac6c9d7f8c40 89 if (_Servo.Ping() == statusValid)
henryrawas 12:ac6c9d7f8c40 90 return true;
henryrawas 12:ac6c9d7f8c40 91 else
henryrawas 12:ac6c9d7f8c40 92 return false;
henryrawas 12:ac6c9d7f8c40 93
henryrawas 12:ac6c9d7f8c40 94 case NA_Init:
henryrawas 15:4bd10f531cdc 95 sc = _Servo.SetReplyDelay(100);
henryrawas 12:ac6c9d7f8c40 96 sc = _Servo.TorqueEnable(false);
henryrawas 12:ac6c9d7f8c40 97 sc = _Servo.TorqueEnable(true);
henryrawas 12:ac6c9d7f8c40 98 return sc == statusValid;
henryrawas 12:ac6c9d7f8c40 99
henryrawas 12:ac6c9d7f8c40 100 case NA_Rotate:
henryrawas 12:ac6c9d7f8c40 101 sc = _Servo.SetGoal(actionValue);
henryrawas 12:ac6c9d7f8c40 102 return sc == statusValid;
henryrawas 12:ac6c9d7f8c40 103
henryrawas 12:ac6c9d7f8c40 104 case NA_ClearError:
henryrawas 12:ac6c9d7f8c40 105 sc = _Servo.TorqueEnable(false);
henryrawas 12:ac6c9d7f8c40 106 sc = _Servo.TorqueEnable(true);
henryrawas 12:ac6c9d7f8c40 107 return sc == statusValid;
henryrawas 12:ac6c9d7f8c40 108
henryrawas 12:ac6c9d7f8c40 109 default:
henryrawas 12:ac6c9d7f8c40 110 return false;
henryrawas 12:ac6c9d7f8c40 111 }
henryrawas 12:ac6c9d7f8c40 112 }
henryrawas 12:ac6c9d7f8c40 113
henryrawas 12:ac6c9d7f8c40 114 NodePartType NodeAX12::GetNodeType()
henryrawas 12:ac6c9d7f8c40 115 {
henryrawas 12:ac6c9d7f8c40 116 return NT_AX12;
henryrawas 12:ac6c9d7f8c40 117 }
henryrawas 12:ac6c9d7f8c40 118
henryrawas 12:ac6c9d7f8c40 119 int NodeAX12::GetLastError()
henryrawas 12:ac6c9d7f8c40 120 {
henryrawas 12:ac6c9d7f8c40 121 return _Servo.GetLastError();
henryrawas 12:ac6c9d7f8c40 122 }
henryrawas 12:ac6c9d7f8c40 123
henryrawas 12:ac6c9d7f8c40 124 bool NodeAX12::HasError()
henryrawas 12:ac6c9d7f8c40 125 {
henryrawas 12:ac6c9d7f8c40 126 return _Servo.HasError();
henryrawas 12:ac6c9d7f8c40 127 }