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@11:8de493bd8922, 2016-01-19 (annotated)

Committer:
henryrawas
Date:
Tue Jan 19 19:48:59 2016 +0000
Revision:
11:8de493bd8922
Parent:
10:e4c9b94b5879
Child:
12:f66c779ca018
Read all values together and cache

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 8:b6979be5a0a7 12 _LastError = statusValid;
henryrawas 11:8de493bd8922 13 _HasReadCache = false;
henryrawas 11:8de493bd8922 14 }
henryrawas 11:8de493bd8922 15
henryrawas 11:8de493bd8922 16 /*****/
henryrawas 11:8de493bd8922 17
henryrawas 11:8de493bd8922 18 void AX12::ClearCache()
henryrawas 11:8de493bd8922 19 {
henryrawas 11:8de493bd8922 20 _HasReadCache = false;
jepickett 0:1a48094c99d1 21 }
jepickett 0:1a48094c99d1 22
jepickett 0:1a48094c99d1 23 /*****/
jepickett 0:1a48094c99d1 24
jepickett 0:1a48094c99d1 25 StatusCode AX12::Ping()
jepickett 0:1a48094c99d1 26 {
jepickett 0:1a48094c99d1 27 return _pbus->Ping(_ID);
jepickett 0:1a48094c99d1 28 }
jepickett 0:1a48094c99d1 29
jepickett 0:1a48094c99d1 30 /*****/
jepickett 0:1a48094c99d1 31
jepickett 2:f2da3b1d9988 32 StatusCode AX12::SetServoId(char newId)
jepickett 2:f2da3b1d9988 33 {
jepickett 2:f2da3b1d9988 34 int offset = ctID;
jepickett 2:f2da3b1d9988 35 CommBuffer data;
jepickett 2:f2da3b1d9988 36 data.push_back( newId );
jepickett 2:f2da3b1d9988 37 StatusCode s = _pbus->Write(_ID, offset, data);
jepickett 2:f2da3b1d9988 38 if( s == statusValid )
jepickett 2:f2da3b1d9988 39 {
jepickett 2:f2da3b1d9988 40 _ID = newId;
jepickett 2:f2da3b1d9988 41 }
jepickett 2:f2da3b1d9988 42 return s;
jepickett 2:f2da3b1d9988 43 }
jepickett 2:f2da3b1d9988 44
jepickett 2:f2da3b1d9988 45 /*****/
jepickett 2:f2da3b1d9988 46
jepickett 0:1a48094c99d1 47 StatusCode AX12::SetGoal(float degrees)
jepickett 0:1a48094c99d1 48 {
henryrawas 3:7b970151bf19 49 _LastError = statusValid;
henryrawas 4:56c593ae9a45 50 short goal = (short)((1024.0f * (degrees - 30.0f)) / 300.0f);
jepickett 0:1a48094c99d1 51
jepickett 0:1a48094c99d1 52 CommBuffer data;
jepickett 0:1a48094c99d1 53 data.push_back( goal & 0xff );
jepickett 0:1a48094c99d1 54 data.push_back( goal >> 8 );
jepickett 0:1a48094c99d1 55
jepickett 0:1a48094c99d1 56 // write the packet, return the error code
jepickett 1:d7642b2e155d 57 int offset = ctGoalPositionL;
jepickett 0:1a48094c99d1 58 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 59 }
jepickett 0:1a48094c99d1 60
jepickett 0:1a48094c99d1 61 /*****/
jepickett 0:1a48094c99d1 62
henryrawas 11:8de493bd8922 63 StatusCode AX12::SetReplyDelay(int us)
jepickett 0:1a48094c99d1 64 {
henryrawas 3:7b970151bf19 65 _LastError = statusValid;
henryrawas 11:8de493bd8922 66 int val = 250;
henryrawas 11:8de493bd8922 67 if (us < 500)
henryrawas 11:8de493bd8922 68 val = ((us + 1) / 2);
henryrawas 11:8de493bd8922 69
jepickett 1:d7642b2e155d 70 CommBuffer data;
henryrawas 11:8de493bd8922 71 data.push_back( val & 0xff );
henryrawas 11:8de493bd8922 72
henryrawas 11:8de493bd8922 73 // write the packet, return the error code
henryrawas 11:8de493bd8922 74 int offset = ctReturnDelayTime;
henryrawas 11:8de493bd8922 75 return _pbus->Write(_ID, offset, data);
henryrawas 11:8de493bd8922 76 }
henryrawas 11:8de493bd8922 77
henryrawas 11:8de493bd8922 78 /*****/
henryrawas 11:8de493bd8922 79
henryrawas 11:8de493bd8922 80 bool AX12::IsMoving(void)
henryrawas 11:8de493bd8922 81 {
henryrawas 11:8de493bd8922 82 if (!_HasReadCache)
jepickett 1:d7642b2e155d 83 {
henryrawas 11:8de493bd8922 84 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 85 if (_LastError != statusValid)
henryrawas 11:8de493bd8922 86 {
henryrawas 11:8de493bd8922 87 return false;
henryrawas 11:8de493bd8922 88 }
jepickett 1:d7642b2e155d 89 }
henryrawas 11:8de493bd8922 90
henryrawas 11:8de493bd8922 91 return _ReadCache[cctMoving] == 1;
jepickett 1:d7642b2e155d 92 }
jepickett 1:d7642b2e155d 93
jepickett 1:d7642b2e155d 94 /*****/
jepickett 1:d7642b2e155d 95
jepickett 1:d7642b2e155d 96 float AX12::GetPosition()
jepickett 1:d7642b2e155d 97 {
henryrawas 11:8de493bd8922 98 if (!_HasReadCache)
jepickett 1:d7642b2e155d 99 {
henryrawas 11:8de493bd8922 100 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 101 if (_LastError != statusValid)
henryrawas 4:56c593ae9a45 102 {
henryrawas 11:8de493bd8922 103 return 0.0f;
henryrawas 4:56c593ae9a45 104 }
jepickett 1:d7642b2e155d 105 }
henryrawas 11:8de493bd8922 106
henryrawas 11:8de493bd8922 107 int16_t value = _ReadCache[cctPresentPositionL] | (_ReadCache[cctPresentPositionH] << 8);
henryrawas 11:8de493bd8922 108 float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
henryrawas 11:8de493bd8922 109 return degrees;
jepickett 0:1a48094c99d1 110 }
jepickett 0:1a48094c99d1 111
jepickett 0:1a48094c99d1 112 /*****/
jepickett 0:1a48094c99d1 113
jepickett 1:d7642b2e155d 114 int AX12::GetTemperature(void)
jepickett 0:1a48094c99d1 115 {
henryrawas 11:8de493bd8922 116 if (!_HasReadCache)
jepickett 1:d7642b2e155d 117 {
henryrawas 11:8de493bd8922 118 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 119 if (_LastError != statusValid)
henryrawas 4:56c593ae9a45 120 {
henryrawas 11:8de493bd8922 121 return 0;
henryrawas 4:56c593ae9a45 122 }
jepickett 1:d7642b2e155d 123 }
henryrawas 11:8de493bd8922 124
henryrawas 11:8de493bd8922 125 return (int)_ReadCache[cctPresentTemperature];
jepickett 0:1a48094c99d1 126 }
jepickett 0:1a48094c99d1 127
jepickett 0:1a48094c99d1 128 /*****/
jepickett 0:1a48094c99d1 129
jepickett 1:d7642b2e155d 130 float AX12::GetSupplyVoltage(void)
jepickett 0:1a48094c99d1 131 {
henryrawas 11:8de493bd8922 132 if (!_HasReadCache)
jepickett 1:d7642b2e155d 133 {
henryrawas 11:8de493bd8922 134 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 135 if (_LastError != statusValid)
henryrawas 4:56c593ae9a45 136 {
henryrawas 11:8de493bd8922 137 return 0.0f;
henryrawas 4:56c593ae9a45 138 }
jepickett 1:d7642b2e155d 139 }
henryrawas 11:8de493bd8922 140
henryrawas 11:8de493bd8922 141 return (float)_ReadCache[cctPresentVoltage] / 10.0f;
jepickett 0:1a48094c99d1 142 }
jepickett 0:1a48094c99d1 143
henryrawas 7:155ecc801119 144 /*****/
henryrawas 7:155ecc801119 145
henryrawas 7:155ecc801119 146 float AX12::GetLoad(void)
henryrawas 7:155ecc801119 147 {
henryrawas 11:8de493bd8922 148 if (!_HasReadCache)
henryrawas 7:155ecc801119 149 {
henryrawas 11:8de493bd8922 150 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 151 if (_LastError != statusValid)
henryrawas 7:155ecc801119 152 {
henryrawas 11:8de493bd8922 153 return 0.0f;
henryrawas 7:155ecc801119 154 }
henryrawas 7:155ecc801119 155 }
henryrawas 11:8de493bd8922 156
henryrawas 11:8de493bd8922 157 int16_t value = _ReadCache[cctPresentLoadL] | (_ReadCache[cctPresentLoadH] << 8);
henryrawas 11:8de493bd8922 158 return (float)(value & 0x03ff);
henryrawas 7:155ecc801119 159 }
henryrawas 7:155ecc801119 160
henryrawas 11:8de493bd8922 161 /*****/
henryrawas 11:8de493bd8922 162
jepickett 1:d7642b2e155d 163 StatusCode AX12::TorqueEnable(bool enable)
jepickett 1:d7642b2e155d 164 {
henryrawas 3:7b970151bf19 165 _LastError = statusValid;
jepickett 1:d7642b2e155d 166 CommBuffer data;
jepickett 1:d7642b2e155d 167 data.push_back( enable ? 1 : 0 );
jepickett 0:1a48094c99d1 168
jepickett 1:d7642b2e155d 169 int offset = ctTorqueEnable;
jepickett 1:d7642b2e155d 170 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 171 }
jepickett 2:f2da3b1d9988 172
henryrawas 11:8de493bd8922 173 /*****/
henryrawas 3:7b970151bf19 174
henryrawas 6:a702043b1420 175 int AX12::GetLastError()
henryrawas 3:7b970151bf19 176 {
henryrawas 10:e4c9b94b5879 177 return _LastError;
henryrawas 3:7b970151bf19 178 }
henryrawas 5:bae6dc62dfb4 179
henryrawas 11:8de493bd8922 180 /*****/
henryrawas 11:8de493bd8922 181
henryrawas 8:b6979be5a0a7 182 bool AX12::HasError()
henryrawas 8:b6979be5a0a7 183 {
henryrawas 8:b6979be5a0a7 184 return _LastError != statusValid;
henryrawas 8:b6979be5a0a7 185 }
henryrawas 11:8de493bd8922 186
henryrawas 11:8de493bd8922 187 /*****/
henryrawas 11:8de493bd8922 188
henryrawas 11:8de493bd8922 189 StatusCode AX12::LoadReadCache(void)
henryrawas 11:8de493bd8922 190 {
henryrawas 11:8de493bd8922 191 _LastError = statusValid;
henryrawas 11:8de493bd8922 192 CommBuffer data;
henryrawas 11:8de493bd8922 193 _HasReadCache = false;
henryrawas 11:8de493bd8922 194
henryrawas 11:8de493bd8922 195 StatusCode s = _pbus->Read(_ID, AX12ReadBlockStart, AX12ReadBlockLength, data);
henryrawas 11:8de493bd8922 196 if( s == statusValid )
henryrawas 11:8de493bd8922 197 {
henryrawas 11:8de493bd8922 198 for (int ix = 0; ix < AX12ReadBlockLength; ix++)
henryrawas 11:8de493bd8922 199 {
henryrawas 11:8de493bd8922 200 _ReadCache[ix] = data[ix];
henryrawas 11:8de493bd8922 201 }
henryrawas 11:8de493bd8922 202 _HasReadCache = true;
henryrawas 11:8de493bd8922 203 }
henryrawas 11:8de493bd8922 204 else
henryrawas 11:8de493bd8922 205 {
henryrawas 11:8de493bd8922 206 _LastError = s;
henryrawas 11:8de493bd8922 207 }
henryrawas 11:8de493bd8922 208 return s;
henryrawas 11:8de493bd8922 209 }