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@2:f2da3b1d9988, 2015-12-17 (annotated)

Committer:
jepickett
Date:
Thu Dec 17 00:09:38 2015 +0000
Revision:
2:f2da3b1d9988
Parent:
1:d7642b2e155d
Child:
5:bae6dc62dfb4
added SetId

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