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@10:e4c9b94b5879, 2016-01-07 (annotated)

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