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@14:9afd50c38c80, 2016-02-02 (annotated)

Committer:
henryrawas
Date:
Tue Feb 02 00:43:49 2016 +0000
Revision:
14:9afd50c38c80
Parent:
13:2e6eb60163f6 
use config file

Who changed what in which revision?

UserRevisionLine numberNew contents of line
henryrawas 14:9afd50c38c80 1 /* Copyright (C) 2016 Schakra Inc, MIT License
henryrawas 12:f66c779ca018 2 *
henryrawas 12:f66c779ca018 3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
henryrawas 12:f66c779ca018 4 * and associated documentation files (the "Software"), to deal in the Software without restriction,
henryrawas 12:f66c779ca018 5 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
henryrawas 12:f66c779ca018 6 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
henryrawas 12:f66c779ca018 7 * furnished to do so, subject to the following conditions:
henryrawas 12:f66c779ca018 8 *
henryrawas 12:f66c779ca018 9 * The above copyright notice and this permission notice shall be included in all copies or
henryrawas 12:f66c779ca018 10 * substantial portions of the Software.
henryrawas 12:f66c779ca018 11 *
henryrawas 12:f66c779ca018 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
henryrawas 12:f66c779ca018 13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
henryrawas 12:f66c779ca018 14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
henryrawas 12:f66c779ca018 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
henryrawas 12:f66c779ca018 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
henryrawas 12:f66c779ca018 17 */
henryrawas 12:f66c779ca018 18
jepickett 0:1a48094c99d1 19 #include "AX12.h"
jepickett 0:1a48094c99d1 20 #include "mbed.h"
henryrawas 3:7b970151bf19 21
jepickett 0:1a48094c99d1 22
jepickett 0:1a48094c99d1 23 AX12::AX12( DynamixelBus* pbus, ServoId ID)
jepickett 0:1a48094c99d1 24 {
jepickett 0:1a48094c99d1 25 _pbus = pbus;
jepickett 0:1a48094c99d1 26 _ID = ID;
henryrawas 8:b6979be5a0a7 27 _LastError = statusValid;
henryrawas 11:8de493bd8922 28 _HasReadCache = false;
henryrawas 11:8de493bd8922 29 }
henryrawas 11:8de493bd8922 30
henryrawas 11:8de493bd8922 31
henryrawas 11:8de493bd8922 32 void AX12::ClearCache()
henryrawas 11:8de493bd8922 33 {
henryrawas 11:8de493bd8922 34 _HasReadCache = false;
jepickett 0:1a48094c99d1 35 }
jepickett 0:1a48094c99d1 36
jepickett 0:1a48094c99d1 37
jepickett 0:1a48094c99d1 38 StatusCode AX12::Ping()
jepickett 0:1a48094c99d1 39 {
jepickett 0:1a48094c99d1 40 return _pbus->Ping(_ID);
jepickett 0:1a48094c99d1 41 }
jepickett 0:1a48094c99d1 42
jepickett 0:1a48094c99d1 43
jepickett 2:f2da3b1d9988 44 StatusCode AX12::SetServoId(char newId)
jepickett 2:f2da3b1d9988 45 {
jepickett 2:f2da3b1d9988 46 int offset = ctID;
jepickett 2:f2da3b1d9988 47 CommBuffer data;
jepickett 2:f2da3b1d9988 48 data.push_back( newId );
jepickett 2:f2da3b1d9988 49 StatusCode s = _pbus->Write(_ID, offset, data);
jepickett 2:f2da3b1d9988 50 if( s == statusValid )
jepickett 2:f2da3b1d9988 51 {
jepickett 2:f2da3b1d9988 52 _ID = newId;
jepickett 2:f2da3b1d9988 53 }
jepickett 2:f2da3b1d9988 54 return s;
jepickett 2:f2da3b1d9988 55 }
jepickett 2:f2da3b1d9988 56
jepickett 2:f2da3b1d9988 57
jepickett 0:1a48094c99d1 58 StatusCode AX12::SetGoal(float degrees)
jepickett 0:1a48094c99d1 59 {
henryrawas 3:7b970151bf19 60 _LastError = statusValid;
henryrawas 4:56c593ae9a45 61 short goal = (short)((1024.0f * (degrees - 30.0f)) / 300.0f);
jepickett 0:1a48094c99d1 62
jepickett 0:1a48094c99d1 63 CommBuffer data;
jepickett 0:1a48094c99d1 64 data.push_back( goal & 0xff );
jepickett 0:1a48094c99d1 65 data.push_back( goal >> 8 );
jepickett 0:1a48094c99d1 66
jepickett 0:1a48094c99d1 67 // write the packet, return the error code
jepickett 1:d7642b2e155d 68 int offset = ctGoalPositionL;
jepickett 0:1a48094c99d1 69 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 70 }
jepickett 0:1a48094c99d1 71
jepickett 0:1a48094c99d1 72
henryrawas 11:8de493bd8922 73 StatusCode AX12::SetReplyDelay(int us)
jepickett 0:1a48094c99d1 74 {
henryrawas 3:7b970151bf19 75 _LastError = statusValid;
henryrawas 11:8de493bd8922 76 int val = 250;
henryrawas 11:8de493bd8922 77 if (us < 500)
henryrawas 11:8de493bd8922 78 val = ((us + 1) / 2);
henryrawas 11:8de493bd8922 79
jepickett 1:d7642b2e155d 80 CommBuffer data;
henryrawas 11:8de493bd8922 81 data.push_back( val & 0xff );
henryrawas 11:8de493bd8922 82
henryrawas 11:8de493bd8922 83 // write the packet, return the error code
henryrawas 11:8de493bd8922 84 int offset = ctReturnDelayTime;
henryrawas 11:8de493bd8922 85 return _pbus->Write(_ID, offset, data);
henryrawas 11:8de493bd8922 86 }
henryrawas 11:8de493bd8922 87
henryrawas 11:8de493bd8922 88
henryrawas 11:8de493bd8922 89 bool AX12::IsMoving(void)
henryrawas 11:8de493bd8922 90 {
henryrawas 11:8de493bd8922 91 if (!_HasReadCache)
jepickett 1:d7642b2e155d 92 {
henryrawas 11:8de493bd8922 93 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 94 if (_LastError != statusValid)
henryrawas 11:8de493bd8922 95 {
henryrawas 11:8de493bd8922 96 return false;
henryrawas 11:8de493bd8922 97 }
jepickett 1:d7642b2e155d 98 }
henryrawas 11:8de493bd8922 99
henryrawas 11:8de493bd8922 100 return _ReadCache[cctMoving] == 1;
jepickett 1:d7642b2e155d 101 }
jepickett 1:d7642b2e155d 102
jepickett 1:d7642b2e155d 103
jepickett 1:d7642b2e155d 104 float AX12::GetPosition()
jepickett 1:d7642b2e155d 105 {
henryrawas 11:8de493bd8922 106 if (!_HasReadCache)
jepickett 1:d7642b2e155d 107 {
henryrawas 11:8de493bd8922 108 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 109 if (_LastError != statusValid)
henryrawas 4:56c593ae9a45 110 {
henryrawas 11:8de493bd8922 111 return 0.0f;
henryrawas 4:56c593ae9a45 112 }
jepickett 1:d7642b2e155d 113 }
henryrawas 11:8de493bd8922 114
henryrawas 11:8de493bd8922 115 int16_t value = _ReadCache[cctPresentPositionL] | (_ReadCache[cctPresentPositionH] << 8);
henryrawas 11:8de493bd8922 116 float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
henryrawas 11:8de493bd8922 117 return degrees;
jepickett 0:1a48094c99d1 118 }
jepickett 0:1a48094c99d1 119
jepickett 0:1a48094c99d1 120
jepickett 1:d7642b2e155d 121 int AX12::GetTemperature(void)
jepickett 0:1a48094c99d1 122 {
henryrawas 11:8de493bd8922 123 if (!_HasReadCache)
jepickett 1:d7642b2e155d 124 {
henryrawas 11:8de493bd8922 125 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 126 if (_LastError != statusValid)
henryrawas 4:56c593ae9a45 127 {
henryrawas 11:8de493bd8922 128 return 0;
henryrawas 4:56c593ae9a45 129 }
jepickett 1:d7642b2e155d 130 }
henryrawas 11:8de493bd8922 131
henryrawas 11:8de493bd8922 132 return (int)_ReadCache[cctPresentTemperature];
jepickett 0:1a48094c99d1 133 }
jepickett 0:1a48094c99d1 134
jepickett 0:1a48094c99d1 135
jepickett 1:d7642b2e155d 136 float AX12::GetSupplyVoltage(void)
jepickett 0:1a48094c99d1 137 {
henryrawas 11:8de493bd8922 138 if (!_HasReadCache)
jepickett 1:d7642b2e155d 139 {
henryrawas 11:8de493bd8922 140 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 141 if (_LastError != statusValid)
henryrawas 4:56c593ae9a45 142 {
henryrawas 11:8de493bd8922 143 return 0.0f;
henryrawas 4:56c593ae9a45 144 }
jepickett 1:d7642b2e155d 145 }
henryrawas 11:8de493bd8922 146
henryrawas 11:8de493bd8922 147 return (float)_ReadCache[cctPresentVoltage] / 10.0f;
jepickett 0:1a48094c99d1 148 }
jepickett 0:1a48094c99d1 149
henryrawas 7:155ecc801119 150
henryrawas 7:155ecc801119 151 float AX12::GetLoad(void)
henryrawas 7:155ecc801119 152 {
henryrawas 11:8de493bd8922 153 if (!_HasReadCache)
henryrawas 7:155ecc801119 154 {
henryrawas 11:8de493bd8922 155 _LastError = LoadReadCache();
henryrawas 11:8de493bd8922 156 if (_LastError != statusValid)
henryrawas 7:155ecc801119 157 {
henryrawas 11:8de493bd8922 158 return 0.0f;
henryrawas 7:155ecc801119 159 }
henryrawas 7:155ecc801119 160 }
henryrawas 11:8de493bd8922 161
henryrawas 11:8de493bd8922 162 int16_t value = _ReadCache[cctPresentLoadL] | (_ReadCache[cctPresentLoadH] << 8);
henryrawas 11:8de493bd8922 163 return (float)(value & 0x03ff);
henryrawas 7:155ecc801119 164 }
henryrawas 7:155ecc801119 165
henryrawas 11:8de493bd8922 166
jepickett 1:d7642b2e155d 167 StatusCode AX12::TorqueEnable(bool enable)
jepickett 1:d7642b2e155d 168 {
henryrawas 3:7b970151bf19 169 _LastError = statusValid;
jepickett 1:d7642b2e155d 170 CommBuffer data;
jepickett 1:d7642b2e155d 171 data.push_back( enable ? 1 : 0 );
jepickett 0:1a48094c99d1 172
jepickett 1:d7642b2e155d 173 int offset = ctTorqueEnable;
jepickett 1:d7642b2e155d 174 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 175 }
jepickett 2:f2da3b1d9988 176
henryrawas 3:7b970151bf19 177
henryrawas 6:a702043b1420 178 int AX12::GetLastError()
henryrawas 3:7b970151bf19 179 {
henryrawas 10:e4c9b94b5879 180 return _LastError;
henryrawas 3:7b970151bf19 181 }
henryrawas 5:bae6dc62dfb4 182
henryrawas 11:8de493bd8922 183
henryrawas 8:b6979be5a0a7 184 bool AX12::HasError()
henryrawas 8:b6979be5a0a7 185 {
henryrawas 8:b6979be5a0a7 186 return _LastError != statusValid;
henryrawas 8:b6979be5a0a7 187 }
henryrawas 11:8de493bd8922 188
henryrawas 11:8de493bd8922 189
henryrawas 11:8de493bd8922 190 StatusCode AX12::LoadReadCache(void)
henryrawas 11:8de493bd8922 191 {
henryrawas 11:8de493bd8922 192 _LastError = statusValid;
henryrawas 11:8de493bd8922 193 CommBuffer data;
henryrawas 11:8de493bd8922 194 _HasReadCache = false;
henryrawas 11:8de493bd8922 195
henryrawas 11:8de493bd8922 196 StatusCode s = _pbus->Read(_ID, AX12ReadBlockStart, AX12ReadBlockLength, data);
henryrawas 11:8de493bd8922 197 if( s == statusValid )
henryrawas 11:8de493bd8922 198 {
henryrawas 11:8de493bd8922 199 for (int ix = 0; ix < AX12ReadBlockLength; ix++)
henryrawas 11:8de493bd8922 200 {
henryrawas 11:8de493bd8922 201 _ReadCache[ix] = data[ix];
henryrawas 11:8de493bd8922 202 }
henryrawas 11:8de493bd8922 203 _HasReadCache = true;
henryrawas 11:8de493bd8922 204 }
henryrawas 11:8de493bd8922 205 else
henryrawas 11:8de493bd8922 206 {
henryrawas 11:8de493bd8922 207 _LastError = s;
henryrawas 11:8de493bd8922 208 }
henryrawas 11:8de493bd8922 209 return s;
henryrawas 11:8de493bd8922 210 }