robot arm demo team / AX-12A

Provides an interface to an AX-12A servo. Requires the Dynamixel bus protocol library/

Fork of AX-12A by Jonathan Pickett

AX12.cpp@7:155ecc801119, 2015-12-31 (annotated)

Committer:
henryrawas
Date:
Thu Dec 31 17:47:43 2015 +0000
Revision:
7:155ecc801119
Parent:
6:a702043b1420
Child:
8:b6979be5a0a7
fixes

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 3:7b970151bf19 5 #include <Terminal.h>
henryrawas 3:7b970151bf19 6
jepickett 0:1a48094c99d1 7
jepickett 0:1a48094c99d1 8 AX12::AX12( DynamixelBus* pbus, ServoId ID)
jepickett 0:1a48094c99d1 9 {
jepickett 0:1a48094c99d1 10 _pbus = pbus;
jepickett 0:1a48094c99d1 11 _ID = ID;
henryrawas 3:7b970151bf19 12 _LastPosition= 0.0f;
henryrawas 3:7b970151bf19 13 _LastTemperature = 0;
henryrawas 3:7b970151bf19 14 _LastVoltage = 0;
henryrawas 6:a702043b1420 15 _LastError = 0;
henryrawas 7:155ecc801119 16 _LastLoad = 0;
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 2:f2da3b1d9988 28 StatusCode AX12::SetServoId(char newId)
jepickett 2:f2da3b1d9988 29 {
jepickett 2:f2da3b1d9988 30 int offset = ctID;
jepickett 2:f2da3b1d9988 31 CommBuffer data;
jepickett 2:f2da3b1d9988 32 data.push_back( newId );
jepickett 2:f2da3b1d9988 33 StatusCode s = _pbus->Write(_ID, offset, data);
jepickett 2:f2da3b1d9988 34 if( s == statusValid )
jepickett 2:f2da3b1d9988 35 {
jepickett 2:f2da3b1d9988 36 _ID = newId;
jepickett 2:f2da3b1d9988 37 }
jepickett 2:f2da3b1d9988 38 return s;
jepickett 2:f2da3b1d9988 39 }
jepickett 2:f2da3b1d9988 40
jepickett 2:f2da3b1d9988 41 /*****/
jepickett 2:f2da3b1d9988 42
jepickett 0:1a48094c99d1 43 StatusCode AX12::SetGoal(float degrees)
jepickett 0:1a48094c99d1 44 {
henryrawas 3:7b970151bf19 45 _LastError = statusValid;
henryrawas 4:56c593ae9a45 46 short goal = (short)((1024.0f * (degrees - 30.0f)) / 300.0f);
jepickett 0:1a48094c99d1 47
jepickett 0:1a48094c99d1 48 CommBuffer data;
jepickett 0:1a48094c99d1 49 data.push_back( goal & 0xff );
jepickett 0:1a48094c99d1 50 data.push_back( goal >> 8 );
jepickett 0:1a48094c99d1 51
jepickett 0:1a48094c99d1 52 // write the packet, return the error code
jepickett 1:d7642b2e155d 53 int offset = ctGoalPositionL;
jepickett 0:1a48094c99d1 54 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 55 }
jepickett 0:1a48094c99d1 56
jepickett 0:1a48094c99d1 57 /*****/
jepickett 0:1a48094c99d1 58
jepickett 1:d7642b2e155d 59 bool AX12::IsMoving(void)
jepickett 0:1a48094c99d1 60 {
henryrawas 3:7b970151bf19 61 _LastError = statusValid;
jepickett 1:d7642b2e155d 62 CommBuffer data;
jepickett 1:d7642b2e155d 63 StatusCode s = _pbus->Read(_ID, ctMoving, 1, data);
jepickett 1:d7642b2e155d 64 if( s == statusValid )
jepickett 1:d7642b2e155d 65 {
jepickett 1:d7642b2e155d 66 return data[0] == 1;
jepickett 1:d7642b2e155d 67 }
jepickett 1:d7642b2e155d 68 else
jepickett 1:d7642b2e155d 69 {
henryrawas 3:7b970151bf19 70 _LastError = s;
jepickett 1:d7642b2e155d 71 return false;
jepickett 1:d7642b2e155d 72 }
jepickett 1:d7642b2e155d 73 }
jepickett 1:d7642b2e155d 74
jepickett 1:d7642b2e155d 75 /*****/
jepickett 1:d7642b2e155d 76
jepickett 1:d7642b2e155d 77 float AX12::GetPosition()
jepickett 1:d7642b2e155d 78 {
henryrawas 3:7b970151bf19 79 _LastError = statusValid;
jepickett 1:d7642b2e155d 80 CommBuffer data;
jepickett 1:d7642b2e155d 81 StatusCode s = _pbus->Read(_ID, ctPresentPositionL, 2, data);
jepickett 1:d7642b2e155d 82 if( s == statusValid )
jepickett 1:d7642b2e155d 83 {
jepickett 1:d7642b2e155d 84 int16_t value = data[0] | (data[1] << 8);
henryrawas 4:56c593ae9a45 85 float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
henryrawas 3:7b970151bf19 86 _LastPosition = degrees;
jepickett 1:d7642b2e155d 87 return degrees;
jepickett 1:d7642b2e155d 88 }
jepickett 1:d7642b2e155d 89 else
jepickett 1:d7642b2e155d 90 {
henryrawas 4:56c593ae9a45 91 // try again one time
henryrawas 4:56c593ae9a45 92 s = _pbus->Read(_ID, ctPresentPositionL, 2, data);
henryrawas 4:56c593ae9a45 93 if( s == statusValid )
henryrawas 4:56c593ae9a45 94 {
henryrawas 4:56c593ae9a45 95 int16_t value = data[0] | (data[1] << 8);
henryrawas 4:56c593ae9a45 96 float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
henryrawas 4:56c593ae9a45 97 _LastPosition = degrees;
henryrawas 4:56c593ae9a45 98 return degrees;
henryrawas 4:56c593ae9a45 99 }
henryrawas 3:7b970151bf19 100 _LastError = s;
jepickett 1:d7642b2e155d 101 return 0.0f;
jepickett 1:d7642b2e155d 102 }
jepickett 0:1a48094c99d1 103 }
jepickett 0:1a48094c99d1 104
jepickett 0:1a48094c99d1 105 /*****/
jepickett 0:1a48094c99d1 106
jepickett 1:d7642b2e155d 107 int AX12::GetTemperature(void)
jepickett 0:1a48094c99d1 108 {
henryrawas 3:7b970151bf19 109 _LastError = statusValid;
jepickett 1:d7642b2e155d 110 CommBuffer data;
jepickett 1:d7642b2e155d 111 StatusCode s = _pbus->Read(_ID, ctPresentTemperature, 1, data);
jepickett 1:d7642b2e155d 112 if( s == statusValid )
jepickett 1:d7642b2e155d 113 {
henryrawas 3:7b970151bf19 114 _LastTemperature = (int)data[0];
jepickett 1:d7642b2e155d 115 return (int)data[0];
jepickett 1:d7642b2e155d 116 }
jepickett 1:d7642b2e155d 117 else
jepickett 1:d7642b2e155d 118 {
henryrawas 4:56c593ae9a45 119 // try again one time
henryrawas 4:56c593ae9a45 120 s = _pbus->Read(_ID, ctPresentTemperature, 1, data);
henryrawas 4:56c593ae9a45 121 if( s == statusValid )
henryrawas 4:56c593ae9a45 122 {
henryrawas 4:56c593ae9a45 123 _LastTemperature = (int)data[0];
henryrawas 4:56c593ae9a45 124 return (int)data[0];
henryrawas 4:56c593ae9a45 125 }
henryrawas 4:56c593ae9a45 126
henryrawas 3:7b970151bf19 127 _LastError = s;
jepickett 1:d7642b2e155d 128 return 0;
jepickett 1:d7642b2e155d 129 }
jepickett 0:1a48094c99d1 130 }
jepickett 0:1a48094c99d1 131
jepickett 0:1a48094c99d1 132 /*****/
jepickett 0:1a48094c99d1 133
jepickett 1:d7642b2e155d 134 float AX12::GetSupplyVoltage(void)
jepickett 0:1a48094c99d1 135 {
henryrawas 3:7b970151bf19 136 _LastError = statusValid;
jepickett 1:d7642b2e155d 137 CommBuffer data;
jepickett 1:d7642b2e155d 138 StatusCode s = _pbus->Read(_ID, ctPresentVoltage, 1, data);
jepickett 1:d7642b2e155d 139 if( s == statusValid )
jepickett 1:d7642b2e155d 140 {
henryrawas 3:7b970151bf19 141 float volts = (float)data[0] / 10.0f;
henryrawas 3:7b970151bf19 142 _LastVoltage = volts;
henryrawas 3:7b970151bf19 143 return volts;
jepickett 1:d7642b2e155d 144 }
jepickett 1:d7642b2e155d 145 else
jepickett 1:d7642b2e155d 146 {
henryrawas 4:56c593ae9a45 147 // try again one time
henryrawas 4:56c593ae9a45 148 s = _pbus->Read(_ID, ctPresentVoltage, 1, data);
henryrawas 4:56c593ae9a45 149 if( s == statusValid )
henryrawas 4:56c593ae9a45 150 {
henryrawas 4:56c593ae9a45 151 float volts = (float)data[0] / 10.0f;
henryrawas 4:56c593ae9a45 152 _LastVoltage = volts;
henryrawas 4:56c593ae9a45 153 return volts;
henryrawas 4:56c593ae9a45 154 }
henryrawas 3:7b970151bf19 155 _LastError = s;
jepickett 1:d7642b2e155d 156 return 0.0f;
jepickett 1:d7642b2e155d 157 }
jepickett 0:1a48094c99d1 158 }
jepickett 0:1a48094c99d1 159
henryrawas 7:155ecc801119 160 /*****/
henryrawas 7:155ecc801119 161
henryrawas 7:155ecc801119 162 float AX12::GetLoad(void)
henryrawas 7:155ecc801119 163 {
henryrawas 7:155ecc801119 164 _LastError = statusValid;
henryrawas 7:155ecc801119 165 CommBuffer data;
henryrawas 7:155ecc801119 166 StatusCode s = _pbus->Read(_ID, ctPresentLoadL, 2, data);
henryrawas 7:155ecc801119 167 if( s == statusValid )
henryrawas 7:155ecc801119 168 {
henryrawas 7:155ecc801119 169 int16_t value = data[0] | (data[1] << 8);
henryrawas 7:155ecc801119 170 _LastLoad = (float)(value & 0x03ff);
henryrawas 7:155ecc801119 171 return _LastLoad;
henryrawas 7:155ecc801119 172 }
henryrawas 7:155ecc801119 173 else
henryrawas 7:155ecc801119 174 {
henryrawas 7:155ecc801119 175 // try again one time
henryrawas 7:155ecc801119 176 s = _pbus->Read(_ID, ctPresentLoadL, 2, data);
henryrawas 7:155ecc801119 177 if( s == statusValid )
henryrawas 7:155ecc801119 178 {
henryrawas 7:155ecc801119 179 int16_t value = data[0] | (data[1] << 8);
henryrawas 7:155ecc801119 180 _LastLoad = (float)(value & 0x03ff);
henryrawas 7:155ecc801119 181 return _LastLoad;
henryrawas 7:155ecc801119 182 }
henryrawas 7:155ecc801119 183 _LastError = s;
henryrawas 7:155ecc801119 184 return 0.0f;
henryrawas 7:155ecc801119 185 }
henryrawas 7:155ecc801119 186 }
henryrawas 7:155ecc801119 187
jepickett 1:d7642b2e155d 188 StatusCode AX12::TorqueEnable(bool enable)
jepickett 1:d7642b2e155d 189 {
henryrawas 3:7b970151bf19 190 _LastError = statusValid;
jepickett 1:d7642b2e155d 191 CommBuffer data;
jepickett 1:d7642b2e155d 192 data.push_back( enable ? 1 : 0 );
jepickett 0:1a48094c99d1 193
jepickett 1:d7642b2e155d 194 int offset = ctTorqueEnable;
jepickett 1:d7642b2e155d 195 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 196 }
jepickett 2:f2da3b1d9988 197
henryrawas 3:7b970151bf19 198 bool AX12::HasMeasure(int measureId)
henryrawas 3:7b970151bf19 199 {
henryrawas 3:7b970151bf19 200 switch (measureId)
henryrawas 3:7b970151bf19 201 {
henryrawas 3:7b970151bf19 202 case NM_Temperature:
henryrawas 3:7b970151bf19 203 return true;
henryrawas 3:7b970151bf19 204
henryrawas 3:7b970151bf19 205 case NM_Degrees:
henryrawas 3:7b970151bf19 206 return true;
henryrawas 3:7b970151bf19 207
henryrawas 3:7b970151bf19 208 case NM_Voltage:
henryrawas 3:7b970151bf19 209 return true;
henryrawas 3:7b970151bf19 210
henryrawas 7:155ecc801119 211 case NM_Load:
henryrawas 7:155ecc801119 212 return true;
henryrawas 7:155ecc801119 213
henryrawas 3:7b970151bf19 214 default:
henryrawas 3:7b970151bf19 215 return false;
henryrawas 3:7b970151bf19 216 }
henryrawas 3:7b970151bf19 217 }
henryrawas 3:7b970151bf19 218
henryrawas 3:7b970151bf19 219 float AX12::GetMeasure(int measureId)
henryrawas 3:7b970151bf19 220 {
henryrawas 3:7b970151bf19 221 switch (measureId)
henryrawas 3:7b970151bf19 222 {
henryrawas 3:7b970151bf19 223 case NM_Temperature:
henryrawas 3:7b970151bf19 224 return (float)GetTemperature();
henryrawas 3:7b970151bf19 225
henryrawas 3:7b970151bf19 226 case NM_Degrees:
henryrawas 3:7b970151bf19 227 return GetPosition();
henryrawas 3:7b970151bf19 228
henryrawas 3:7b970151bf19 229 case NM_Voltage:
henryrawas 3:7b970151bf19 230 return GetSupplyVoltage();
henryrawas 3:7b970151bf19 231
henryrawas 7:155ecc801119 232 case NM_Load:
henryrawas 7:155ecc801119 233 return GetLoad();
henryrawas 7:155ecc801119 234
henryrawas 3:7b970151bf19 235 default:
henryrawas 3:7b970151bf19 236 return 0.0f;
henryrawas 3:7b970151bf19 237 }
henryrawas 3:7b970151bf19 238 }
henryrawas 3:7b970151bf19 239
henryrawas 3:7b970151bf19 240 float AX12::GetLastMeasure(int measureId)
henryrawas 3:7b970151bf19 241 {
henryrawas 3:7b970151bf19 242 switch (measureId)
henryrawas 3:7b970151bf19 243 {
henryrawas 3:7b970151bf19 244 case NM_Temperature:
henryrawas 3:7b970151bf19 245 return (float)_LastTemperature;
henryrawas 3:7b970151bf19 246
henryrawas 3:7b970151bf19 247 case NM_Degrees:
henryrawas 3:7b970151bf19 248 return _LastPosition;
henryrawas 3:7b970151bf19 249
henryrawas 3:7b970151bf19 250 case NM_Voltage:
henryrawas 3:7b970151bf19 251 return _LastVoltage;
henryrawas 3:7b970151bf19 252
henryrawas 7:155ecc801119 253 case NM_Load:
henryrawas 7:155ecc801119 254 return _LastLoad;
henryrawas 7:155ecc801119 255
henryrawas 3:7b970151bf19 256 default:
henryrawas 3:7b970151bf19 257 return 0.0f;
henryrawas 3:7b970151bf19 258 }
henryrawas 3:7b970151bf19 259 }
henryrawas 3:7b970151bf19 260
henryrawas 3:7b970151bf19 261 bool AX12::HasAction(int actionId)
henryrawas 3:7b970151bf19 262 {
henryrawas 3:7b970151bf19 263 switch (actionId)
henryrawas 3:7b970151bf19 264 {
henryrawas 3:7b970151bf19 265 case NA_Rotate:
henryrawas 3:7b970151bf19 266 return true;
henryrawas 3:7b970151bf19 267
henryrawas 3:7b970151bf19 268 default:
henryrawas 3:7b970151bf19 269 return false;
henryrawas 3:7b970151bf19 270 }
henryrawas 3:7b970151bf19 271 }
henryrawas 3:7b970151bf19 272
henryrawas 3:7b970151bf19 273 // TODO: error code handling cleanup
henryrawas 3:7b970151bf19 274 bool AX12::DoAction(int actionId, float actionValue)
henryrawas 3:7b970151bf19 275 {
henryrawas 3:7b970151bf19 276 _LastError = statusValid;
henryrawas 3:7b970151bf19 277 switch (actionId)
henryrawas 3:7b970151bf19 278 {
henryrawas 3:7b970151bf19 279 case NA_Rotate:
henryrawas 3:7b970151bf19 280 _LastError = SetGoal(actionValue);
henryrawas 3:7b970151bf19 281 return _LastError == statusValid;
henryrawas 3:7b970151bf19 282
henryrawas 3:7b970151bf19 283 default:
henryrawas 3:7b970151bf19 284 return instructionError;
henryrawas 3:7b970151bf19 285 }
henryrawas 3:7b970151bf19 286 }
henryrawas 3:7b970151bf19 287
henryrawas 3:7b970151bf19 288 NodePartType AX12::GetNodeType()
henryrawas 3:7b970151bf19 289 {
henryrawas 3:7b970151bf19 290 return NT_AX12;
henryrawas 3:7b970151bf19 291 }
henryrawas 3:7b970151bf19 292
henryrawas 6:a702043b1420 293 int AX12::GetLastError()
henryrawas 3:7b970151bf19 294 {
henryrawas 6:a702043b1420 295 return (int)_LastError;
henryrawas 3:7b970151bf19 296 }
henryrawas 5:bae6dc62dfb4 297