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@5:bae6dc62dfb4, 2015-12-28 (annotated)

Committer:
henryrawas
Date:
Mon Dec 28 17:28:16 2015 +0000
Revision:
5:bae6dc62dfb4
Parent:
3:56c593ae9a45
Parent:
4:f2da3b1d9988
Child:
6:a702043b1420
merge RobotNode support

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 4:f2da3b1d9988 28 StatusCode AX12::SetServoId(char newId)
jepickett 4:f2da3b1d9988 29 {
jepickett 4:f2da3b1d9988 30 int offset = ctID;
jepickett 4:f2da3b1d9988 31 CommBuffer data;
jepickett 4:f2da3b1d9988 32 data.push_back( newId );
jepickett 4:f2da3b1d9988 33 StatusCode s = _pbus->Write(_ID, offset, data);
jepickett 4:f2da3b1d9988 34 if( s == statusValid )
jepickett 4:f2da3b1d9988 35 {
jepickett 4:f2da3b1d9988 36 _ID = newId;
jepickett 4:f2da3b1d9988 37 }
jepickett 4:f2da3b1d9988 38 return s;
jepickett 4:f2da3b1d9988 39 }
jepickett 4:f2da3b1d9988 40
jepickett 4:f2da3b1d9988 41 /*****/
jepickett 4:f2da3b1d9988 42
jepickett 0:1a48094c99d1 43 StatusCode AX12::SetGoal(float degrees)
jepickett 0:1a48094c99d1 44 {
henryrawas 2:7b970151bf19 45 _LastError = statusValid;
henryrawas 3: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 2: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 2: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 2: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 3:56c593ae9a45 85 float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
henryrawas 2:7b970151bf19 86 _LastPosition = degrees;
jepickett 1:d7642b2e155d 87 return degrees;
jepickett 1:d7642b2e155d 88 }
jepickett 1:d7642b2e155d 89 else
jepickett 1:d7642b2e155d 90 {
henryrawas 3:56c593ae9a45 91 // try again one time
henryrawas 3:56c593ae9a45 92 s = _pbus->Read(_ID, ctPresentPositionL, 2, data);
henryrawas 3:56c593ae9a45 93 if( s == statusValid )
henryrawas 3:56c593ae9a45 94 {
henryrawas 3:56c593ae9a45 95 int16_t value = data[0] | (data[1] << 8);
henryrawas 3:56c593ae9a45 96 float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
henryrawas 3:56c593ae9a45 97 _LastPosition = degrees;
henryrawas 3:56c593ae9a45 98 return degrees;
henryrawas 3:56c593ae9a45 99 }
henryrawas 2: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 2: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 2: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 3:56c593ae9a45 119 // try again one time
henryrawas 3:56c593ae9a45 120 s = _pbus->Read(_ID, ctPresentTemperature, 1, data);
henryrawas 3:56c593ae9a45 121 if( s == statusValid )
henryrawas 3:56c593ae9a45 122 {
henryrawas 3:56c593ae9a45 123 _LastTemperature = (int)data[0];
henryrawas 3:56c593ae9a45 124 return (int)data[0];
henryrawas 3:56c593ae9a45 125 }
henryrawas 3:56c593ae9a45 126
henryrawas 2: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 2: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 2:7b970151bf19 141 float volts = (float)data[0] / 10.0f;
henryrawas 2:7b970151bf19 142 _LastVoltage = volts;
henryrawas 2:7b970151bf19 143 return volts;
jepickett 1:d7642b2e155d 144 }
jepickett 1:d7642b2e155d 145 else
jepickett 1:d7642b2e155d 146 {
henryrawas 3:56c593ae9a45 147 // try again one time
henryrawas 3:56c593ae9a45 148 s = _pbus->Read(_ID, ctPresentVoltage, 1, data);
henryrawas 3:56c593ae9a45 149 if( s == statusValid )
henryrawas 3:56c593ae9a45 150 {
henryrawas 3:56c593ae9a45 151 float volts = (float)data[0] / 10.0f;
henryrawas 3:56c593ae9a45 152 _LastVoltage = volts;
henryrawas 3:56c593ae9a45 153 return volts;
henryrawas 3:56c593ae9a45 154 }
henryrawas 2:7b970151bf19 155 _LastError = s;
jepickett 1:d7642b2e155d 156 return 0.0f;
jepickett 1:d7642b2e155d 157 }
jepickett 0:1a48094c99d1 158 }
jepickett 0:1a48094c99d1 159
jepickett 1:d7642b2e155d 160 StatusCode AX12::TorqueEnable(bool enable)
jepickett 1:d7642b2e155d 161 {
henryrawas 2:7b970151bf19 162 _LastError = statusValid;
jepickett 1:d7642b2e155d 163 CommBuffer data;
jepickett 1:d7642b2e155d 164 data.push_back( enable ? 1 : 0 );
jepickett 0:1a48094c99d1 165
jepickett 1:d7642b2e155d 166 int offset = ctTorqueEnable;
jepickett 1:d7642b2e155d 167 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 168 }
jepickett 4:f2da3b1d9988 169
henryrawas 2:7b970151bf19 170 bool AX12::HasMeasure(int measureId)
henryrawas 2:7b970151bf19 171 {
henryrawas 2:7b970151bf19 172 switch (measureId)
henryrawas 2:7b970151bf19 173 {
henryrawas 2:7b970151bf19 174 case NM_Temperature:
henryrawas 2:7b970151bf19 175 return true;
henryrawas 2:7b970151bf19 176
henryrawas 2:7b970151bf19 177 case NM_Degrees:
henryrawas 2:7b970151bf19 178 return true;
henryrawas 2:7b970151bf19 179
henryrawas 2:7b970151bf19 180 case NM_Voltage:
henryrawas 2:7b970151bf19 181 return true;
henryrawas 2:7b970151bf19 182
henryrawas 2:7b970151bf19 183 default:
henryrawas 2:7b970151bf19 184 return false;
henryrawas 2:7b970151bf19 185 }
henryrawas 2:7b970151bf19 186 }
henryrawas 2:7b970151bf19 187
henryrawas 2:7b970151bf19 188 float AX12::GetMeasure(int measureId)
henryrawas 2:7b970151bf19 189 {
henryrawas 2:7b970151bf19 190 switch (measureId)
henryrawas 2:7b970151bf19 191 {
henryrawas 2:7b970151bf19 192 case NM_Temperature:
henryrawas 2:7b970151bf19 193 return (float)GetTemperature();
henryrawas 2:7b970151bf19 194
henryrawas 2:7b970151bf19 195 case NM_Degrees:
henryrawas 2:7b970151bf19 196 return GetPosition();
henryrawas 2:7b970151bf19 197
henryrawas 2:7b970151bf19 198 case NM_Voltage:
henryrawas 2:7b970151bf19 199 return GetSupplyVoltage();
henryrawas 2:7b970151bf19 200
henryrawas 2:7b970151bf19 201 default:
henryrawas 2:7b970151bf19 202 return 0.0f;
henryrawas 2:7b970151bf19 203 }
henryrawas 2:7b970151bf19 204 }
henryrawas 2:7b970151bf19 205
henryrawas 2:7b970151bf19 206 float AX12::GetLastMeasure(int measureId)
henryrawas 2:7b970151bf19 207 {
henryrawas 2:7b970151bf19 208 switch (measureId)
henryrawas 2:7b970151bf19 209 {
henryrawas 2:7b970151bf19 210 case NM_Temperature:
henryrawas 2:7b970151bf19 211 return (float)_LastTemperature;
henryrawas 2:7b970151bf19 212
henryrawas 2:7b970151bf19 213 case NM_Degrees:
henryrawas 2:7b970151bf19 214 return _LastPosition;
henryrawas 2:7b970151bf19 215
henryrawas 2:7b970151bf19 216 case NM_Voltage:
henryrawas 2:7b970151bf19 217 return _LastVoltage;
henryrawas 2:7b970151bf19 218
henryrawas 2:7b970151bf19 219 default:
henryrawas 2:7b970151bf19 220 return 0.0f;
henryrawas 2:7b970151bf19 221 }
henryrawas 2:7b970151bf19 222 }
henryrawas 2:7b970151bf19 223
henryrawas 2:7b970151bf19 224 bool AX12::HasAction(int actionId)
henryrawas 2:7b970151bf19 225 {
henryrawas 2:7b970151bf19 226 switch (actionId)
henryrawas 2:7b970151bf19 227 {
henryrawas 2:7b970151bf19 228 case NA_Rotate:
henryrawas 2:7b970151bf19 229 return true;
henryrawas 2:7b970151bf19 230
henryrawas 2:7b970151bf19 231 default:
henryrawas 2:7b970151bf19 232 return false;
henryrawas 2:7b970151bf19 233 }
henryrawas 2:7b970151bf19 234 }
henryrawas 2:7b970151bf19 235
henryrawas 2:7b970151bf19 236 // TODO: error code handling cleanup
henryrawas 2:7b970151bf19 237 bool AX12::DoAction(int actionId, float actionValue)
henryrawas 2:7b970151bf19 238 {
henryrawas 2:7b970151bf19 239 _LastError = statusValid;
henryrawas 2:7b970151bf19 240 switch (actionId)
henryrawas 2:7b970151bf19 241 {
henryrawas 2:7b970151bf19 242 case NA_Rotate:
henryrawas 2:7b970151bf19 243 _LastError = SetGoal(actionValue);
henryrawas 2:7b970151bf19 244 return _LastError == statusValid;
henryrawas 2:7b970151bf19 245
henryrawas 2:7b970151bf19 246 default:
henryrawas 2:7b970151bf19 247 return instructionError;
henryrawas 2:7b970151bf19 248 }
henryrawas 2:7b970151bf19 249 }
henryrawas 2:7b970151bf19 250
henryrawas 2:7b970151bf19 251 NodePartType AX12::GetNodeType()
henryrawas 2:7b970151bf19 252 {
henryrawas 2:7b970151bf19 253 return NT_AX12;
henryrawas 2:7b970151bf19 254 }
henryrawas 2:7b970151bf19 255
henryrawas 2:7b970151bf19 256 unsigned char AX12::GetLastError()
henryrawas 2:7b970151bf19 257 {
henryrawas 2:7b970151bf19 258 return _LastError;
henryrawas 2:7b970151bf19 259 }
henryrawas 5:bae6dc62dfb4 260