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@1:d7642b2e155d, 2015-12-15 (annotated)

Committer:
jepickett
Date:
Tue Dec 15 04:37:54 2015 +0000
Revision:
1:d7642b2e155d
Parent:
0:1a48094c99d1
Child:
2:f2da3b1d9988
Child:
3:7b970151bf19
propagating changes from ServoTerminal

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"
jepickett 0:1a48094c99d1 5
jepickett 0:1a48094c99d1 6 AX12::AX12( DynamixelBus* pbus, ServoId ID)
jepickett 0:1a48094c99d1 7 {
jepickett 0:1a48094c99d1 8 _pbus = pbus;
jepickett 0:1a48094c99d1 9 _ID = ID;
jepickett 0:1a48094c99d1 10 }
jepickett 0:1a48094c99d1 11
jepickett 0:1a48094c99d1 12 /*****/
jepickett 0:1a48094c99d1 13
jepickett 0:1a48094c99d1 14 StatusCode AX12::Ping()
jepickett 0:1a48094c99d1 15 {
jepickett 0:1a48094c99d1 16 return _pbus->Ping(_ID);
jepickett 0:1a48094c99d1 17 }
jepickett 0:1a48094c99d1 18
jepickett 0:1a48094c99d1 19 /*****/
jepickett 0:1a48094c99d1 20
jepickett 0:1a48094c99d1 21 StatusCode AX12::SetGoal(float degrees)
jepickett 0:1a48094c99d1 22 {
jepickett 1:d7642b2e155d 23 short goal = (short)((1024.0f * degrees) / 300.0f);
jepickett 0:1a48094c99d1 24
jepickett 0:1a48094c99d1 25 CommBuffer data;
jepickett 0:1a48094c99d1 26 data.push_back( goal & 0xff );
jepickett 0:1a48094c99d1 27 data.push_back( goal >> 8 );
jepickett 0:1a48094c99d1 28
jepickett 0:1a48094c99d1 29 // write the packet, return the error code
jepickett 1:d7642b2e155d 30 int offset = ctGoalPositionL;
jepickett 0:1a48094c99d1 31 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 32 }
jepickett 0:1a48094c99d1 33
jepickett 0:1a48094c99d1 34 /*****/
jepickett 0:1a48094c99d1 35
jepickett 1:d7642b2e155d 36 bool AX12::IsMoving(void)
jepickett 0:1a48094c99d1 37 {
jepickett 1:d7642b2e155d 38 CommBuffer data;
jepickett 1:d7642b2e155d 39 StatusCode s = _pbus->Read(_ID, ctMoving, 1, data);
jepickett 1:d7642b2e155d 40 if( s == statusValid )
jepickett 1:d7642b2e155d 41 {
jepickett 1:d7642b2e155d 42 return data[0] == 1;
jepickett 1:d7642b2e155d 43 }
jepickett 1:d7642b2e155d 44 else
jepickett 1:d7642b2e155d 45 {
jepickett 1:d7642b2e155d 46 return false;
jepickett 1:d7642b2e155d 47 }
jepickett 1:d7642b2e155d 48 }
jepickett 1:d7642b2e155d 49
jepickett 1:d7642b2e155d 50 /*****/
jepickett 1:d7642b2e155d 51
jepickett 1:d7642b2e155d 52 float AX12::GetPosition()
jepickett 1:d7642b2e155d 53 {
jepickett 1:d7642b2e155d 54 CommBuffer data;
jepickett 1:d7642b2e155d 55 StatusCode s = _pbus->Read(_ID, ctPresentPositionL, 2, data);
jepickett 1:d7642b2e155d 56 if( s == statusValid )
jepickett 1:d7642b2e155d 57 {
jepickett 1:d7642b2e155d 58 int16_t value = data[0] | (data[1] << 8);
jepickett 1:d7642b2e155d 59 float degrees = (float)value * 300.0f / 1024.0f;
jepickett 1:d7642b2e155d 60 return degrees;
jepickett 1:d7642b2e155d 61 }
jepickett 1:d7642b2e155d 62 else
jepickett 1:d7642b2e155d 63 {
jepickett 1:d7642b2e155d 64 return 0.0f;
jepickett 1:d7642b2e155d 65 }
jepickett 0:1a48094c99d1 66 }
jepickett 0:1a48094c99d1 67
jepickett 0:1a48094c99d1 68 /*****/
jepickett 0:1a48094c99d1 69
jepickett 1:d7642b2e155d 70 int AX12::GetTemperature(void)
jepickett 0:1a48094c99d1 71 {
jepickett 1:d7642b2e155d 72 CommBuffer data;
jepickett 1:d7642b2e155d 73 StatusCode s = _pbus->Read(_ID, ctPresentTemperature, 1, data);
jepickett 1:d7642b2e155d 74 if( s == statusValid )
jepickett 1:d7642b2e155d 75 {
jepickett 1:d7642b2e155d 76 return (int)data[0];
jepickett 1:d7642b2e155d 77 }
jepickett 1:d7642b2e155d 78 else
jepickett 1:d7642b2e155d 79 {
jepickett 1:d7642b2e155d 80 return 0;
jepickett 1:d7642b2e155d 81 }
jepickett 0:1a48094c99d1 82 }
jepickett 0:1a48094c99d1 83
jepickett 0:1a48094c99d1 84 /*****/
jepickett 0:1a48094c99d1 85
jepickett 1:d7642b2e155d 86 float AX12::GetSupplyVoltage(void)
jepickett 0:1a48094c99d1 87 {
jepickett 1:d7642b2e155d 88 CommBuffer data;
jepickett 1:d7642b2e155d 89 StatusCode s = _pbus->Read(_ID, ctPresentVoltage, 1, data);
jepickett 1:d7642b2e155d 90 if( s == statusValid )
jepickett 1:d7642b2e155d 91 {
jepickett 1:d7642b2e155d 92 return (float)data[0] / 10.0f;
jepickett 1:d7642b2e155d 93 }
jepickett 1:d7642b2e155d 94 else
jepickett 1:d7642b2e155d 95 {
jepickett 1:d7642b2e155d 96 return 0.0f;
jepickett 1:d7642b2e155d 97 }
jepickett 0:1a48094c99d1 98 }
jepickett 0:1a48094c99d1 99
jepickett 1:d7642b2e155d 100 StatusCode AX12::TorqueEnable(bool enable)
jepickett 1:d7642b2e155d 101 {
jepickett 1:d7642b2e155d 102 CommBuffer data;
jepickett 1:d7642b2e155d 103 data.push_back( enable ? 1 : 0 );
jepickett 0:1a48094c99d1 104
jepickett 1:d7642b2e155d 105 int offset = ctTorqueEnable;
jepickett 1:d7642b2e155d 106 return _pbus->Write(_ID, offset, data);
jepickett 0:1a48094c99d1 107 }