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@12:ac6c9d7f8c40, 2016-01-07 (annotated)

Committer:
henryrawas
Date:
Thu Jan 07 17:31:23 2016 +0000
Revision:
12:ac6c9d7f8c40
Child:
13:ffeff9b5e513
refactor robotnode

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 12:ac6c9d7f8c40 9 _LastPosition= 0.0f;
henryrawas 12:ac6c9d7f8c40 10 _LastTemperature = 0;
henryrawas 12:ac6c9d7f8c40 11 _LastVoltage = 0;
henryrawas 12:ac6c9d7f8c40 12 _LastLoad = 0;
henryrawas 12:ac6c9d7f8c40 13 }
henryrawas 12:ac6c9d7f8c40 14
henryrawas 12:ac6c9d7f8c40 15
henryrawas 12:ac6c9d7f8c40 16 bool NodeAX12::HasMeasure(int measureId)
henryrawas 12:ac6c9d7f8c40 17 {
henryrawas 12:ac6c9d7f8c40 18 switch (measureId)
henryrawas 12:ac6c9d7f8c40 19 {
henryrawas 12:ac6c9d7f8c40 20 case NM_Temperature:
henryrawas 12:ac6c9d7f8c40 21 return true;
henryrawas 12:ac6c9d7f8c40 22
henryrawas 12:ac6c9d7f8c40 23 case NM_Degrees:
henryrawas 12:ac6c9d7f8c40 24 return true;
henryrawas 12:ac6c9d7f8c40 25
henryrawas 12:ac6c9d7f8c40 26 case NM_Voltage:
henryrawas 12:ac6c9d7f8c40 27 return true;
henryrawas 12:ac6c9d7f8c40 28
henryrawas 12:ac6c9d7f8c40 29 case NM_Load:
henryrawas 12:ac6c9d7f8c40 30 return true;
henryrawas 12:ac6c9d7f8c40 31
henryrawas 12:ac6c9d7f8c40 32 default:
henryrawas 12:ac6c9d7f8c40 33 return false;
henryrawas 12:ac6c9d7f8c40 34 }
henryrawas 12:ac6c9d7f8c40 35 }
henryrawas 12:ac6c9d7f8c40 36
henryrawas 12:ac6c9d7f8c40 37 float NodeAX12::GetMeasure(int measureId)
henryrawas 12:ac6c9d7f8c40 38 {
henryrawas 12:ac6c9d7f8c40 39 float val;
henryrawas 12:ac6c9d7f8c40 40 switch (measureId)
henryrawas 12:ac6c9d7f8c40 41 {
henryrawas 12:ac6c9d7f8c40 42 case NM_Temperature:
henryrawas 12:ac6c9d7f8c40 43 val = (float)_Servo.GetTemperature();
henryrawas 12:ac6c9d7f8c40 44 if (val != 0.0f)
henryrawas 12:ac6c9d7f8c40 45 _LastTemperature = val;
henryrawas 12:ac6c9d7f8c40 46 return val;
henryrawas 12:ac6c9d7f8c40 47
henryrawas 12:ac6c9d7f8c40 48 case NM_Degrees:
henryrawas 12:ac6c9d7f8c40 49 val = (float)_Servo.GetPosition();
henryrawas 12:ac6c9d7f8c40 50 if (val != 0.0f)
henryrawas 12:ac6c9d7f8c40 51 _LastPosition = val;
henryrawas 12:ac6c9d7f8c40 52 return val;
henryrawas 12:ac6c9d7f8c40 53
henryrawas 12:ac6c9d7f8c40 54 case NM_Voltage:
henryrawas 12:ac6c9d7f8c40 55 val = (float)_Servo.GetSupplyVoltage();
henryrawas 12:ac6c9d7f8c40 56 if (val != 0.0f)
henryrawas 12:ac6c9d7f8c40 57 _LastVoltage = val;
henryrawas 12:ac6c9d7f8c40 58 return val;
henryrawas 12:ac6c9d7f8c40 59
henryrawas 12:ac6c9d7f8c40 60 case NM_Load:
henryrawas 12:ac6c9d7f8c40 61 val = (float)_Servo.GetLoad();
henryrawas 12:ac6c9d7f8c40 62 if (val != 0.0f)
henryrawas 12:ac6c9d7f8c40 63 _LastLoad = val;
henryrawas 12:ac6c9d7f8c40 64 return val;
henryrawas 12:ac6c9d7f8c40 65
henryrawas 12:ac6c9d7f8c40 66 default:
henryrawas 12:ac6c9d7f8c40 67 return 0.0f;
henryrawas 12:ac6c9d7f8c40 68 }
henryrawas 12:ac6c9d7f8c40 69 }
henryrawas 12:ac6c9d7f8c40 70
henryrawas 12:ac6c9d7f8c40 71 float NodeAX12::GetLastMeasure(int measureId)
henryrawas 12:ac6c9d7f8c40 72 {
henryrawas 12:ac6c9d7f8c40 73 switch (measureId)
henryrawas 12:ac6c9d7f8c40 74 {
henryrawas 12:ac6c9d7f8c40 75 case NM_Temperature:
henryrawas 12:ac6c9d7f8c40 76 return (float)_LastTemperature;
henryrawas 12:ac6c9d7f8c40 77
henryrawas 12:ac6c9d7f8c40 78 case NM_Degrees:
henryrawas 12:ac6c9d7f8c40 79 return _LastPosition;
henryrawas 12:ac6c9d7f8c40 80
henryrawas 12:ac6c9d7f8c40 81 case NM_Voltage:
henryrawas 12:ac6c9d7f8c40 82 return _LastVoltage;
henryrawas 12:ac6c9d7f8c40 83
henryrawas 12:ac6c9d7f8c40 84 case NM_Load:
henryrawas 12:ac6c9d7f8c40 85 return _LastLoad;
henryrawas 12:ac6c9d7f8c40 86
henryrawas 12:ac6c9d7f8c40 87 default:
henryrawas 12:ac6c9d7f8c40 88 return 0.0f;
henryrawas 12:ac6c9d7f8c40 89 }
henryrawas 12:ac6c9d7f8c40 90 }
henryrawas 12:ac6c9d7f8c40 91
henryrawas 12:ac6c9d7f8c40 92 bool NodeAX12::HasAction(int actionId)
henryrawas 12:ac6c9d7f8c40 93 {
henryrawas 12:ac6c9d7f8c40 94 switch (actionId)
henryrawas 12:ac6c9d7f8c40 95 {
henryrawas 12:ac6c9d7f8c40 96 case NA_Ping:
henryrawas 12:ac6c9d7f8c40 97 return true;
henryrawas 12:ac6c9d7f8c40 98
henryrawas 12:ac6c9d7f8c40 99 case NA_Init:
henryrawas 12:ac6c9d7f8c40 100 return true;
henryrawas 12:ac6c9d7f8c40 101
henryrawas 12:ac6c9d7f8c40 102 case NA_Rotate:
henryrawas 12:ac6c9d7f8c40 103 return true;
henryrawas 12:ac6c9d7f8c40 104
henryrawas 12:ac6c9d7f8c40 105 case NA_ClearError:
henryrawas 12:ac6c9d7f8c40 106 return true;
henryrawas 12:ac6c9d7f8c40 107
henryrawas 12:ac6c9d7f8c40 108 default:
henryrawas 12:ac6c9d7f8c40 109 return false;
henryrawas 12:ac6c9d7f8c40 110 }
henryrawas 12:ac6c9d7f8c40 111 }
henryrawas 12:ac6c9d7f8c40 112
henryrawas 12:ac6c9d7f8c40 113
henryrawas 12:ac6c9d7f8c40 114 bool NodeAX12::DoAction(int actionId, float actionValue)
henryrawas 12:ac6c9d7f8c40 115 {
henryrawas 12:ac6c9d7f8c40 116 StatusCode sc;
henryrawas 12:ac6c9d7f8c40 117
henryrawas 12:ac6c9d7f8c40 118 switch (actionId)
henryrawas 12:ac6c9d7f8c40 119 {
henryrawas 12:ac6c9d7f8c40 120 case NA_Ping:
henryrawas 12:ac6c9d7f8c40 121 if (_Servo.Ping() == statusValid)
henryrawas 12:ac6c9d7f8c40 122 return true;
henryrawas 12:ac6c9d7f8c40 123 else
henryrawas 12:ac6c9d7f8c40 124 return false;
henryrawas 12:ac6c9d7f8c40 125
henryrawas 12:ac6c9d7f8c40 126 case NA_Init:
henryrawas 12:ac6c9d7f8c40 127 sc = _Servo.TorqueEnable(false);
henryrawas 12:ac6c9d7f8c40 128 sc = _Servo.TorqueEnable(true);
henryrawas 12:ac6c9d7f8c40 129 return sc == statusValid;
henryrawas 12:ac6c9d7f8c40 130
henryrawas 12:ac6c9d7f8c40 131 case NA_Rotate:
henryrawas 12:ac6c9d7f8c40 132 sc = _Servo.SetGoal(actionValue);
henryrawas 12:ac6c9d7f8c40 133 return sc == statusValid;
henryrawas 12:ac6c9d7f8c40 134
henryrawas 12:ac6c9d7f8c40 135 case NA_ClearError:
henryrawas 12:ac6c9d7f8c40 136 sc = _Servo.TorqueEnable(false);
henryrawas 12:ac6c9d7f8c40 137 sc = _Servo.TorqueEnable(true);
henryrawas 12:ac6c9d7f8c40 138 return sc == statusValid;
henryrawas 12:ac6c9d7f8c40 139
henryrawas 12:ac6c9d7f8c40 140 default:
henryrawas 12:ac6c9d7f8c40 141 return false;
henryrawas 12:ac6c9d7f8c40 142 }
henryrawas 12:ac6c9d7f8c40 143 }
henryrawas 12:ac6c9d7f8c40 144
henryrawas 12:ac6c9d7f8c40 145 NodePartType NodeAX12::GetNodeType()
henryrawas 12:ac6c9d7f8c40 146 {
henryrawas 12:ac6c9d7f8c40 147 return NT_AX12;
henryrawas 12:ac6c9d7f8c40 148 }
henryrawas 12:ac6c9d7f8c40 149
henryrawas 12:ac6c9d7f8c40 150 int NodeAX12::GetLastError()
henryrawas 12:ac6c9d7f8c40 151 {
henryrawas 12:ac6c9d7f8c40 152 return _Servo.GetLastError();
henryrawas 12:ac6c9d7f8c40 153 }
henryrawas 12:ac6c9d7f8c40 154
henryrawas 12:ac6c9d7f8c40 155 bool NodeAX12::HasError()
henryrawas 12:ac6c9d7f8c40 156 {
henryrawas 12:ac6c9d7f8c40 157 return _Servo.HasError();
henryrawas 12:ac6c9d7f8c40 158 }