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@12:f66c779ca018, 2016-01-23 (annotated)

Committer:
henryrawas
Date:
Sat Jan 23 00:08:04 2016 +0000
Revision:
12:f66c779ca018
Parent:
11:8de493bd8922
Child:
13:2e6eb60163f6
refactor

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