Telesensorics / AX-12A

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   RobotArmDemo

Fork of AX-12A by robot arm demo team

AX12.cpp@3:56c593ae9a45, 2015-12-28 (annotated)

Committer:
henryrawas
Date:
Mon Dec 28 17:19:02 2015 +0000
Revision:
3:56c593ae9a45
Parent:
2:7b970151bf19
Child:
5:bae6dc62dfb4
support RobotNode;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jepickett 0:1a48094c99d1 1 /* mbed AX-12+ Servo Library - External hardware version */
jepickett 0:1a48094c99d1 2
jepickett 0:1a48094c99d1 3 #include "AX12.h"
jepickett 0:1a48094c99d1 4 #include "mbed.h"
henryrawas 2:7b970151bf19 5 #include <Terminal.h>
henryrawas 2:7b970151bf19 6
henryrawas 2:7b970151bf19 7 Terminal* AX12Pc = NULL;
jepickett 0:1a48094c99d1 8
jepickett 0:1a48094c99d1 9 AX12::AX12( DynamixelBus* pbus, ServoId ID)
jepickett 0:1a48094c99d1 10 {
jepickett 0:1a48094c99d1 11 _pbus = pbus;
jepickett 0:1a48094c99d1 12 _ID = ID;
henryrawas 2:7b970151bf19 13 _LastPosition= 0.0f;
henryrawas 2:7b970151bf19 14 _LastTemperature = 0;
henryrawas 2:7b970151bf19 15 _LastVoltage = 0;
henryrawas 2:7b970151bf19 16 _LastError = statusValid;
jepickett 0:1a48094c99d1 17 }
jepickett 0:1a48094c99d1 18
jepickett 0:1a48094c99d1 19 /*****/
jepickett 0:1a48094c99d1 20
jepickett 0:1a48094c99d1 21 StatusCode AX12::Ping()
jepickett 0:1a48094c99d1 22 {
jepickett 0:1a48094c99d1 23 return _pbus->Ping(_ID);
jepickett 0:1a48094c99d1 24 }
jepickett 0:1a48094c99d1 25
jepickett 0:1a48094c99d1 26 /*****/
jepickett 0:1a48094c99d1 27
jepickett 0:1a48094c99d1 28 StatusCode AX12::SetGoal(float degrees)
jepickett 0:1a48094c99d1 29 {
henryrawas 2:7b970151bf19 30 _LastError = statusValid;
henryrawas 3:56c593ae9a45 31 short goal = (short)((1024.0f * (degrees - 30.0f)) / 300.0f);
jepickett 0:1a48094c99d1 32
jepickett 0:1a48094c99d1 33 CommBuffer data;
jepickett 0:1a48094c99d1 34 data.push_back( goal & 0xff );
jepickett 0:1a48094c99d1 35 data.push_back( goal >> 8 );
jepickett 0:1a48094c99d1 36
jepickett 0:1a48094c99d1 37 // write the packet, return the error code
jepickett 1:d7642b2e155d 38 int offset = ctGoalPositionL;
jepickett 0:1a48094c99d1 39 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 40 }
jepickett 0:1a48094c99d1 41
jepickett 0:1a48094c99d1 42 /*****/
jepickett 0:1a48094c99d1 43
jepickett 1:d7642b2e155d 44 bool AX12::IsMoving(void)
jepickett 0:1a48094c99d1 45 {
henryrawas 2:7b970151bf19 46 _LastError = statusValid;
jepickett 1:d7642b2e155d 47 CommBuffer data;
jepickett 1:d7642b2e155d 48 StatusCode s = _pbus->Read(_ID, ctMoving, 1, data);
jepickett 1:d7642b2e155d 49 if( s == statusValid )
jepickett 1:d7642b2e155d 50 {
jepickett 1:d7642b2e155d 51 return data[0] == 1;
jepickett 1:d7642b2e155d 52 }
jepickett 1:d7642b2e155d 53 else
jepickett 1:d7642b2e155d 54 {
henryrawas 2:7b970151bf19 55 _LastError = s;
jepickett 1:d7642b2e155d 56 return false;
jepickett 1:d7642b2e155d 57 }
jepickett 1:d7642b2e155d 58 }
jepickett 1:d7642b2e155d 59
jepickett 1:d7642b2e155d 60 /*****/
jepickett 1:d7642b2e155d 61
jepickett 1:d7642b2e155d 62 float AX12::GetPosition()
jepickett 1:d7642b2e155d 63 {
henryrawas 2:7b970151bf19 64 _LastError = statusValid;
jepickett 1:d7642b2e155d 65 CommBuffer data;
jepickett 1:d7642b2e155d 66 StatusCode s = _pbus->Read(_ID, ctPresentPositionL, 2, data);
jepickett 1:d7642b2e155d 67 if( s == statusValid )
jepickett 1:d7642b2e155d 68 {
jepickett 1:d7642b2e155d 69 int16_t value = data[0] | (data[1] << 8);
henryrawas 3:56c593ae9a45 70 float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
henryrawas 2:7b970151bf19 71 _LastPosition = degrees;
jepickett 1:d7642b2e155d 72 return degrees;
jepickett 1:d7642b2e155d 73 }
jepickett 1:d7642b2e155d 74 else
jepickett 1:d7642b2e155d 75 {
henryrawas 3:56c593ae9a45 76 // try again one time
henryrawas 3:56c593ae9a45 77 s = _pbus->Read(_ID, ctPresentPositionL, 2, data);
henryrawas 3:56c593ae9a45 78 if( s == statusValid )
henryrawas 3:56c593ae9a45 79 {
henryrawas 3:56c593ae9a45 80 int16_t value = data[0] | (data[1] << 8);
henryrawas 3:56c593ae9a45 81 float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
henryrawas 3:56c593ae9a45 82 _LastPosition = degrees;
henryrawas 3:56c593ae9a45 83 return degrees;
henryrawas 3:56c593ae9a45 84 }
henryrawas 2:7b970151bf19 85 _LastError = s;
jepickett 1:d7642b2e155d 86 return 0.0f;
jepickett 1:d7642b2e155d 87 }
jepickett 0:1a48094c99d1 88 }
jepickett 0:1a48094c99d1 89
jepickett 0:1a48094c99d1 90 /*****/
jepickett 0:1a48094c99d1 91
jepickett 1:d7642b2e155d 92 int AX12::GetTemperature(void)
jepickett 0:1a48094c99d1 93 {
henryrawas 2:7b970151bf19 94 _LastError = statusValid;
jepickett 1:d7642b2e155d 95 CommBuffer data;
jepickett 1:d7642b2e155d 96 StatusCode s = _pbus->Read(_ID, ctPresentTemperature, 1, data);
jepickett 1:d7642b2e155d 97 if( s == statusValid )
jepickett 1:d7642b2e155d 98 {
henryrawas 2:7b970151bf19 99 _LastTemperature = (int)data[0];
jepickett 1:d7642b2e155d 100 return (int)data[0];
jepickett 1:d7642b2e155d 101 }
jepickett 1:d7642b2e155d 102 else
jepickett 1:d7642b2e155d 103 {
henryrawas 3:56c593ae9a45 104 // try again one time
henryrawas 3:56c593ae9a45 105 s = _pbus->Read(_ID, ctPresentTemperature, 1, data);
henryrawas 3:56c593ae9a45 106 if( s == statusValid )
henryrawas 3:56c593ae9a45 107 {
henryrawas 3:56c593ae9a45 108 _LastTemperature = (int)data[0];
henryrawas 3:56c593ae9a45 109 return (int)data[0];
henryrawas 3:56c593ae9a45 110 }
henryrawas 3:56c593ae9a45 111
henryrawas 2:7b970151bf19 112 _LastError = s;
jepickett 1:d7642b2e155d 113 return 0;
jepickett 1:d7642b2e155d 114 }
jepickett 0:1a48094c99d1 115 }
jepickett 0:1a48094c99d1 116
jepickett 0:1a48094c99d1 117 /*****/
jepickett 0:1a48094c99d1 118
jepickett 1:d7642b2e155d 119 float AX12::GetSupplyVoltage(void)
jepickett 0:1a48094c99d1 120 {
henryrawas 2:7b970151bf19 121 _LastError = statusValid;
jepickett 1:d7642b2e155d 122 CommBuffer data;
jepickett 1:d7642b2e155d 123 StatusCode s = _pbus->Read(_ID, ctPresentVoltage, 1, data);
jepickett 1:d7642b2e155d 124 if( s == statusValid )
jepickett 1:d7642b2e155d 125 {
henryrawas 2:7b970151bf19 126 float volts = (float)data[0] / 10.0f;
henryrawas 2:7b970151bf19 127 _LastVoltage = volts;
henryrawas 2:7b970151bf19 128 return volts;
jepickett 1:d7642b2e155d 129 }
jepickett 1:d7642b2e155d 130 else
jepickett 1:d7642b2e155d 131 {
henryrawas 3:56c593ae9a45 132 // try again one time
henryrawas 3:56c593ae9a45 133 s = _pbus->Read(_ID, ctPresentVoltage, 1, data);
henryrawas 3:56c593ae9a45 134 if( s == statusValid )
henryrawas 3:56c593ae9a45 135 {
henryrawas 3:56c593ae9a45 136 float volts = (float)data[0] / 10.0f;
henryrawas 3:56c593ae9a45 137 _LastVoltage = volts;
henryrawas 3:56c593ae9a45 138 return volts;
henryrawas 3:56c593ae9a45 139 }
henryrawas 2:7b970151bf19 140 _LastError = s;
jepickett 1:d7642b2e155d 141 return 0.0f;
jepickett 1:d7642b2e155d 142 }
jepickett 0:1a48094c99d1 143 }
jepickett 0:1a48094c99d1 144
jepickett 1:d7642b2e155d 145 StatusCode AX12::TorqueEnable(bool enable)
jepickett 1:d7642b2e155d 146 {
henryrawas 2:7b970151bf19 147 _LastError = statusValid;
jepickett 1:d7642b2e155d 148 CommBuffer data;
jepickett 1:d7642b2e155d 149 data.push_back( enable ? 1 : 0 );
jepickett 0:1a48094c99d1 150
jepickett 1:d7642b2e155d 151 int offset = ctTorqueEnable;
jepickett 1:d7642b2e155d 152 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 153 }
henryrawas 2:7b970151bf19 154
henryrawas 2:7b970151bf19 155 bool AX12::HasMeasure(int measureId)
henryrawas 2:7b970151bf19 156 {
henryrawas 2:7b970151bf19 157 switch (measureId)
henryrawas 2:7b970151bf19 158 {
henryrawas 2:7b970151bf19 159 case NM_Temperature:
henryrawas 2:7b970151bf19 160 return true;
henryrawas 2:7b970151bf19 161
henryrawas 2:7b970151bf19 162 case NM_Degrees:
henryrawas 2:7b970151bf19 163 return true;
henryrawas 2:7b970151bf19 164
henryrawas 2:7b970151bf19 165 case NM_Voltage:
henryrawas 2:7b970151bf19 166 return true;
henryrawas 2:7b970151bf19 167
henryrawas 2:7b970151bf19 168 default:
henryrawas 2:7b970151bf19 169 return false;
henryrawas 2:7b970151bf19 170 }
henryrawas 2:7b970151bf19 171 }
henryrawas 2:7b970151bf19 172
henryrawas 2:7b970151bf19 173 float AX12::GetMeasure(int measureId)
henryrawas 2:7b970151bf19 174 {
henryrawas 2:7b970151bf19 175 switch (measureId)
henryrawas 2:7b970151bf19 176 {
henryrawas 2:7b970151bf19 177 case NM_Temperature:
henryrawas 2:7b970151bf19 178 return (float)GetTemperature();
henryrawas 2:7b970151bf19 179
henryrawas 2:7b970151bf19 180 case NM_Degrees:
henryrawas 2:7b970151bf19 181 return GetPosition();
henryrawas 2:7b970151bf19 182
henryrawas 2:7b970151bf19 183 case NM_Voltage:
henryrawas 2:7b970151bf19 184 return GetSupplyVoltage();
henryrawas 2:7b970151bf19 185
henryrawas 2:7b970151bf19 186 default:
henryrawas 2:7b970151bf19 187 return 0.0f;
henryrawas 2:7b970151bf19 188 }
henryrawas 2:7b970151bf19 189 }
henryrawas 2:7b970151bf19 190
henryrawas 2:7b970151bf19 191 float AX12::GetLastMeasure(int measureId)
henryrawas 2:7b970151bf19 192 {
henryrawas 2:7b970151bf19 193 switch (measureId)
henryrawas 2:7b970151bf19 194 {
henryrawas 2:7b970151bf19 195 case NM_Temperature:
henryrawas 2:7b970151bf19 196 return (float)_LastTemperature;
henryrawas 2:7b970151bf19 197
henryrawas 2:7b970151bf19 198 case NM_Degrees:
henryrawas 2:7b970151bf19 199 return _LastPosition;
henryrawas 2:7b970151bf19 200
henryrawas 2:7b970151bf19 201 case NM_Voltage:
henryrawas 2:7b970151bf19 202 return _LastVoltage;
henryrawas 2:7b970151bf19 203
henryrawas 2:7b970151bf19 204 default:
henryrawas 2:7b970151bf19 205 return 0.0f;
henryrawas 2:7b970151bf19 206 }
henryrawas 2:7b970151bf19 207 }
henryrawas 2:7b970151bf19 208
henryrawas 2:7b970151bf19 209 bool AX12::HasAction(int actionId)
henryrawas 2:7b970151bf19 210 {
henryrawas 2:7b970151bf19 211 switch (actionId)
henryrawas 2:7b970151bf19 212 {
henryrawas 2:7b970151bf19 213 case NA_Rotate:
henryrawas 2:7b970151bf19 214 return true;
henryrawas 2:7b970151bf19 215
henryrawas 2:7b970151bf19 216 default:
henryrawas 2:7b970151bf19 217 return false;
henryrawas 2:7b970151bf19 218 }
henryrawas 2:7b970151bf19 219 }
henryrawas 2:7b970151bf19 220
henryrawas 2:7b970151bf19 221 // TODO: error code handling cleanup
henryrawas 2:7b970151bf19 222 bool AX12::DoAction(int actionId, float actionValue)
henryrawas 2:7b970151bf19 223 {
henryrawas 2:7b970151bf19 224 _LastError = statusValid;
henryrawas 2:7b970151bf19 225 switch (actionId)
henryrawas 2:7b970151bf19 226 {
henryrawas 2:7b970151bf19 227 case NA_Rotate:
henryrawas 2:7b970151bf19 228 _LastError = SetGoal(actionValue);
henryrawas 2:7b970151bf19 229 return _LastError == statusValid;
henryrawas 2:7b970151bf19 230
henryrawas 2:7b970151bf19 231 default:
henryrawas 2:7b970151bf19 232 return instructionError;
henryrawas 2:7b970151bf19 233 }
henryrawas 2:7b970151bf19 234 }
henryrawas 2:7b970151bf19 235
henryrawas 2:7b970151bf19 236 NodePartType AX12::GetNodeType()
henryrawas 2:7b970151bf19 237 {
henryrawas 2:7b970151bf19 238 return NT_AX12;
henryrawas 2:7b970151bf19 239 }
henryrawas 2:7b970151bf19 240
henryrawas 2:7b970151bf19 241 unsigned char AX12::GetLastError()
henryrawas 2:7b970151bf19 242 {
henryrawas 2:7b970151bf19 243 return _LastError;
henryrawas 2:7b970151bf19 244 }