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@13:2e6eb60163f6, 2016-02-01 (annotated)

Committer:
henryrawas
Date:
Mon Feb 01 21:55:45 2016 +0000
Revision:
13:2e6eb60163f6
Parent:
12:f66c779ca018
Child:
14:9afd50c38c80
remove terminal

Who changed what in which revision?

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