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@2:7b970151bf19, 2015-12-23 (annotated)

Committer:
henryrawas
Date:
Wed Dec 23 18:32:37 2015 +0000
Revision:
2:7b970151bf19
Parent:
1:d7642b2e155d
Child:
3:56c593ae9a45
make it derive from 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;
jepickett 1:d7642b2e155d 31 short goal = (short)((1024.0f * degrees) / 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);
jepickett 1:d7642b2e155d 70 float degrees = (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 2:7b970151bf19 76 _LastError = s;
jepickett 1:d7642b2e155d 77 return 0.0f;
jepickett 1:d7642b2e155d 78 }
jepickett 0:1a48094c99d1 79 }
jepickett 0:1a48094c99d1 80
jepickett 0:1a48094c99d1 81 /*****/
jepickett 0:1a48094c99d1 82
jepickett 1:d7642b2e155d 83 int AX12::GetTemperature(void)
jepickett 0:1a48094c99d1 84 {
henryrawas 2:7b970151bf19 85 _LastError = statusValid;
jepickett 1:d7642b2e155d 86 CommBuffer data;
jepickett 1:d7642b2e155d 87 StatusCode s = _pbus->Read(_ID, ctPresentTemperature, 1, data);
jepickett 1:d7642b2e155d 88 if( s == statusValid )
jepickett 1:d7642b2e155d 89 {
henryrawas 2:7b970151bf19 90 _LastTemperature = (int)data[0];
jepickett 1:d7642b2e155d 91 return (int)data[0];
jepickett 1:d7642b2e155d 92 }
jepickett 1:d7642b2e155d 93 else
jepickett 1:d7642b2e155d 94 {
henryrawas 2:7b970151bf19 95 _LastError = s;
henryrawas 2:7b970151bf19 96 _LastTemperature = -1;
jepickett 1:d7642b2e155d 97 return 0;
jepickett 1:d7642b2e155d 98 }
jepickett 0:1a48094c99d1 99 }
jepickett 0:1a48094c99d1 100
jepickett 0:1a48094c99d1 101 /*****/
jepickett 0:1a48094c99d1 102
jepickett 1:d7642b2e155d 103 float AX12::GetSupplyVoltage(void)
jepickett 0:1a48094c99d1 104 {
henryrawas 2:7b970151bf19 105 _LastError = statusValid;
jepickett 1:d7642b2e155d 106 CommBuffer data;
jepickett 1:d7642b2e155d 107 StatusCode s = _pbus->Read(_ID, ctPresentVoltage, 1, data);
jepickett 1:d7642b2e155d 108 if( s == statusValid )
jepickett 1:d7642b2e155d 109 {
henryrawas 2:7b970151bf19 110 float volts = (float)data[0] / 10.0f;
henryrawas 2:7b970151bf19 111 _LastVoltage = volts;
henryrawas 2:7b970151bf19 112 return volts;
jepickett 1:d7642b2e155d 113 }
jepickett 1:d7642b2e155d 114 else
jepickett 1:d7642b2e155d 115 {
henryrawas 2:7b970151bf19 116 _LastError = s;
henryrawas 2:7b970151bf19 117 _LastVoltage = -1;
jepickett 1:d7642b2e155d 118 return 0.0f;
jepickett 1:d7642b2e155d 119 }
jepickett 0:1a48094c99d1 120 }
jepickett 0:1a48094c99d1 121
jepickett 1:d7642b2e155d 122 StatusCode AX12::TorqueEnable(bool enable)
jepickett 1:d7642b2e155d 123 {
henryrawas 2:7b970151bf19 124 _LastError = statusValid;
jepickett 1:d7642b2e155d 125 CommBuffer data;
jepickett 1:d7642b2e155d 126 data.push_back( enable ? 1 : 0 );
jepickett 0:1a48094c99d1 127
jepickett 1:d7642b2e155d 128 int offset = ctTorqueEnable;
jepickett 1:d7642b2e155d 129 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 130 }
henryrawas 2:7b970151bf19 131
henryrawas 2:7b970151bf19 132 bool AX12::HasMeasure(int measureId)
henryrawas 2:7b970151bf19 133 {
henryrawas 2:7b970151bf19 134 switch (measureId)
henryrawas 2:7b970151bf19 135 {
henryrawas 2:7b970151bf19 136 case NM_Temperature:
henryrawas 2:7b970151bf19 137 return true;
henryrawas 2:7b970151bf19 138
henryrawas 2:7b970151bf19 139 case NM_Degrees:
henryrawas 2:7b970151bf19 140 return true;
henryrawas 2:7b970151bf19 141
henryrawas 2:7b970151bf19 142 case NM_Voltage:
henryrawas 2:7b970151bf19 143 return true;
henryrawas 2:7b970151bf19 144
henryrawas 2:7b970151bf19 145 default:
henryrawas 2:7b970151bf19 146 return false;
henryrawas 2:7b970151bf19 147 }
henryrawas 2:7b970151bf19 148 }
henryrawas 2:7b970151bf19 149
henryrawas 2:7b970151bf19 150 float AX12::GetMeasure(int measureId)
henryrawas 2:7b970151bf19 151 {
henryrawas 2:7b970151bf19 152 switch (measureId)
henryrawas 2:7b970151bf19 153 {
henryrawas 2:7b970151bf19 154 case NM_Temperature:
henryrawas 2:7b970151bf19 155 return (float)GetTemperature();
henryrawas 2:7b970151bf19 156
henryrawas 2:7b970151bf19 157 case NM_Degrees:
henryrawas 2:7b970151bf19 158 return GetPosition();
henryrawas 2:7b970151bf19 159
henryrawas 2:7b970151bf19 160 case NM_Voltage:
henryrawas 2:7b970151bf19 161 return GetSupplyVoltage();
henryrawas 2:7b970151bf19 162
henryrawas 2:7b970151bf19 163 default:
henryrawas 2:7b970151bf19 164 return 0.0f;
henryrawas 2:7b970151bf19 165 }
henryrawas 2:7b970151bf19 166 }
henryrawas 2:7b970151bf19 167
henryrawas 2:7b970151bf19 168 float AX12::GetLastMeasure(int measureId)
henryrawas 2:7b970151bf19 169 {
henryrawas 2:7b970151bf19 170 switch (measureId)
henryrawas 2:7b970151bf19 171 {
henryrawas 2:7b970151bf19 172 case NM_Temperature:
henryrawas 2:7b970151bf19 173 return (float)_LastTemperature;
henryrawas 2:7b970151bf19 174
henryrawas 2:7b970151bf19 175 case NM_Degrees:
henryrawas 2:7b970151bf19 176 return _LastPosition;
henryrawas 2:7b970151bf19 177
henryrawas 2:7b970151bf19 178 case NM_Voltage:
henryrawas 2:7b970151bf19 179 return _LastVoltage;
henryrawas 2:7b970151bf19 180
henryrawas 2:7b970151bf19 181 default:
henryrawas 2:7b970151bf19 182 return 0.0f;
henryrawas 2:7b970151bf19 183 }
henryrawas 2:7b970151bf19 184 }
henryrawas 2:7b970151bf19 185
henryrawas 2:7b970151bf19 186 bool AX12::HasAction(int actionId)
henryrawas 2:7b970151bf19 187 {
henryrawas 2:7b970151bf19 188 switch (actionId)
henryrawas 2:7b970151bf19 189 {
henryrawas 2:7b970151bf19 190 case NA_Rotate:
henryrawas 2:7b970151bf19 191 return true;
henryrawas 2:7b970151bf19 192
henryrawas 2:7b970151bf19 193 default:
henryrawas 2:7b970151bf19 194 return false;
henryrawas 2:7b970151bf19 195 }
henryrawas 2:7b970151bf19 196 }
henryrawas 2:7b970151bf19 197
henryrawas 2:7b970151bf19 198 // TODO: error code handling cleanup
henryrawas 2:7b970151bf19 199 bool AX12::DoAction(int actionId, float actionValue)
henryrawas 2:7b970151bf19 200 {
henryrawas 2:7b970151bf19 201 _LastError = statusValid;
henryrawas 2:7b970151bf19 202 switch (actionId)
henryrawas 2:7b970151bf19 203 {
henryrawas 2:7b970151bf19 204 case NA_Rotate:
henryrawas 2:7b970151bf19 205 _LastError = SetGoal(actionValue);
henryrawas 2:7b970151bf19 206 return _LastError == statusValid;
henryrawas 2:7b970151bf19 207
henryrawas 2:7b970151bf19 208 default:
henryrawas 2:7b970151bf19 209 return instructionError;
henryrawas 2:7b970151bf19 210 }
henryrawas 2:7b970151bf19 211 }
henryrawas 2:7b970151bf19 212
henryrawas 2:7b970151bf19 213 NodePartType AX12::GetNodeType()
henryrawas 2:7b970151bf19 214 {
henryrawas 2:7b970151bf19 215 return NT_AX12;
henryrawas 2:7b970151bf19 216 }
henryrawas 2:7b970151bf19 217
henryrawas 2:7b970151bf19 218 unsigned char AX12::GetLastError()
henryrawas 2:7b970151bf19 219 {
henryrawas 2:7b970151bf19 220 return _LastError;
henryrawas 2:7b970151bf19 221 }