William McColl / Mbed 2 deprecated Balanced-Arm

Motor control for robotic arm

Dependencies:   MCP23017 WattBob_TextLCD mbed

Fork of Balanced Arm by Balanced Arm

AX12.cpp@24:462afd19d12e, 2016-03-03 (annotated)

Committer:
wm70
Date:
Thu Mar 03 12:07:25 2016 +0000
Revision:
24:462afd19d12e
Parent:
23:c1c20aee64a0
Child:
25:d345ef4cb2b5
Position - yes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wm70 9:87558e7cfecb 1 #include "AX12.h"
wm70 9:87558e7cfecb 2 #include "mbed.h"
ADAMSTRUTT 11:7f74fb17c328 3 #include "string"
wm70 9:87558e7cfecb 4
wm70 14:3be8ca0c066d 5 AX12::AX12(Serial& bus, Serial& PCbus, PinName dir, int ID)
wm70 14:3be8ca0c066d 6 : _bus(bus), _PCbus(PCbus), _dir(dir){
wm70 9:87558e7cfecb 7
wm70 9:87558e7cfecb 8 _bus.baud(1000000);
wm70 9:87558e7cfecb 9 _ID = ID;
wm70 9:87558e7cfecb 10 _dir = TRANSMIT;
wm70 9:87558e7cfecb 11 }
wm70 9:87558e7cfecb 12
ADAMSTRUTT 23:c1c20aee64a0 13 int AX12::SetGoal(int degrees, int speed){
wm70 18:b79a9a7cf5fb 14
wm70 18:b79a9a7cf5fb 15 short goal = (1023*degrees)/300;
ADAMSTRUTT 23:c1c20aee64a0 16 char data[4];
wm70 9:87558e7cfecb 17 data[0] = goal & 0xff;
wm70 9:87558e7cfecb 18 data[1] = goal >> 8;
ADAMSTRUTT 23:c1c20aee64a0 19 data[2] = 0x00;
ADAMSTRUTT 23:c1c20aee64a0 20 data[3] = 0x00;
ADAMSTRUTT 23:c1c20aee64a0 21
wm70 18:b79a9a7cf5fb 22
ADAMSTRUTT 23:c1c20aee64a0 23
ADAMSTRUTT 23:c1c20aee64a0 24 int rVal = write(_ID, AX12_REG_GOAL_POSITION, 4, data);
ADAMSTRUTT 23:c1c20aee64a0 25
ADAMSTRUTT 23:c1c20aee64a0 26 return (rVal);
ADAMSTRUTT 23:c1c20aee64a0 27 }
ADAMSTRUTT 23:c1c20aee64a0 28
ADAMSTRUTT 23:c1c20aee64a0 29 // Set the mode of the servo
ADAMSTRUTT 23:c1c20aee64a0 30 // 0 = Positional (0-300 degrees)
ADAMSTRUTT 23:c1c20aee64a0 31 // 1 = Rotational -1 to 1 speed
wm70 18:b79a9a7cf5fb 32
ADAMSTRUTT 23:c1c20aee64a0 33 int AX12::SetMode(int mode) {
ADAMSTRUTT 23:c1c20aee64a0 34
ADAMSTRUTT 23:c1c20aee64a0 35 if (mode == 1) {
ADAMSTRUTT 23:c1c20aee64a0 36
ADAMSTRUTT 23:c1c20aee64a0 37 SetCWLimit(0);
ADAMSTRUTT 23:c1c20aee64a0 38 SetCCWLimit(0);
ADAMSTRUTT 23:c1c20aee64a0 39 SetCRSpeed(0.0);
ADAMSTRUTT 23:c1c20aee64a0 40 } else {
ADAMSTRUTT 23:c1c20aee64a0 41 SetCWLimit(70);
ADAMSTRUTT 23:c1c20aee64a0 42 SetCCWLimit(270);
ADAMSTRUTT 23:c1c20aee64a0 43 SetCRSpeed(0.0);
wm70 18:b79a9a7cf5fb 44 }
ADAMSTRUTT 23:c1c20aee64a0 45 return(0);
ADAMSTRUTT 23:c1c20aee64a0 46 }
ADAMSTRUTT 23:c1c20aee64a0 47
ADAMSTRUTT 23:c1c20aee64a0 48 int AX12::SetTorqueLimit (int torque) {
ADAMSTRUTT 23:c1c20aee64a0 49
ADAMSTRUTT 23:c1c20aee64a0 50 char data[2];
wm70 24:462afd19d12e 51 int set = (torque*1023)/10;
wm70 24:462afd19d12e 52
wm70 24:462afd19d12e 53 data[0] = set && 0xff; // bottom 8 bits
wm70 24:462afd19d12e 54 data[1] = set >> 8; // top 8 bits
wm70 24:462afd19d12e 55
ADAMSTRUTT 23:c1c20aee64a0 56
ADAMSTRUTT 23:c1c20aee64a0 57 // write the packet, return the error code
ADAMSTRUTT 23:c1c20aee64a0 58 return (write(_ID, AX12_REG_TORQUE_LIMIT, 2, data));
wm70 24:462afd19d12e 59
wm70 24:462afd19d12e 60 }
wm70 24:462afd19d12e 61
wm70 24:462afd19d12e 62 int AX12::TorqueEnable (int enable) {
wm70 24:462afd19d12e 63
wm70 24:462afd19d12e 64 char data[1];
wm70 24:462afd19d12e 65
wm70 24:462afd19d12e 66 data[0] = enable;
wm70 24:462afd19d12e 67
wm70 24:462afd19d12e 68 return (write(_ID, AX12_REG_TORQUE_ENABLE, 1, data));
wm70 24:462afd19d12e 69
wm70 18:b79a9a7cf5fb 70 }
wm70 9:87558e7cfecb 71
wm70 18:b79a9a7cf5fb 72 int AX12::SetCWLimit (int degrees) {
wm70 18:b79a9a7cf5fb 73
wm70 18:b79a9a7cf5fb 74 char data[2];
wm70 18:b79a9a7cf5fb 75 // 1023 / 300 * degrees
wm70 18:b79a9a7cf5fb 76 short limit = (1023 * degrees) / 300;
wm70 18:b79a9a7cf5fb 77
wm70 18:b79a9a7cf5fb 78 data[0] = limit & 0xff; // bottom 8 bits
wm70 18:b79a9a7cf5fb 79 data[1] = limit >> 8; // top 8 bits
wm70 18:b79a9a7cf5fb 80
wm70 18:b79a9a7cf5fb 81 // write the packet, return the error code
wm70 18:b79a9a7cf5fb 82 return (write(_ID, AX12_REG_CW_LIMIT, 2, data));
wm70 18:b79a9a7cf5fb 83 }
wm70 18:b79a9a7cf5fb 84
wm70 18:b79a9a7cf5fb 85 int AX12::SetCCWLimit (int degrees) {
wm70 18:b79a9a7cf5fb 86
wm70 18:b79a9a7cf5fb 87 char data[2];
wm70 18:b79a9a7cf5fb 88
wm70 18:b79a9a7cf5fb 89 // 1023 / 300 * degrees
wm70 18:b79a9a7cf5fb 90 short limit = (1023 * degrees) / 300;
wm70 18:b79a9a7cf5fb 91
wm70 18:b79a9a7cf5fb 92 data[0] = limit & 0xff; // bottom 8 bits
wm70 18:b79a9a7cf5fb 93 data[1] = limit >> 8; // top 8 bits
wm70 18:b79a9a7cf5fb 94
wm70 18:b79a9a7cf5fb 95 // write the packet, return the error code
wm70 18:b79a9a7cf5fb 96 return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
wm70 18:b79a9a7cf5fb 97 }
wm70 18:b79a9a7cf5fb 98
ADAMSTRUTT 23:c1c20aee64a0 99 int AX12::SetCRSpeed(float speed) {
ADAMSTRUTT 23:c1c20aee64a0 100
ADAMSTRUTT 23:c1c20aee64a0 101 // bit 10 = direction, 0 = CCW, 1=CW
ADAMSTRUTT 23:c1c20aee64a0 102 // bits 9-0 = Speed
ADAMSTRUTT 23:c1c20aee64a0 103 char data[2];
ADAMSTRUTT 23:c1c20aee64a0 104
ADAMSTRUTT 23:c1c20aee64a0 105 int goal = (0x3ff * abs(speed));
ADAMSTRUTT 23:c1c20aee64a0 106
ADAMSTRUTT 23:c1c20aee64a0 107 // Set direction CW if we have a negative speed
ADAMSTRUTT 23:c1c20aee64a0 108 if (speed < 0) {
ADAMSTRUTT 23:c1c20aee64a0 109 goal |= (0x1 << 10);
ADAMSTRUTT 23:c1c20aee64a0 110 }
ADAMSTRUTT 23:c1c20aee64a0 111
ADAMSTRUTT 23:c1c20aee64a0 112 data[0] = goal & 0xff; // bottom 8 bits
ADAMSTRUTT 23:c1c20aee64a0 113 data[1] = goal >> 8; // top 8 bits
ADAMSTRUTT 23:c1c20aee64a0 114
ADAMSTRUTT 23:c1c20aee64a0 115 // write the packet, return the error code
ADAMSTRUTT 23:c1c20aee64a0 116 int rVal = write(_ID, 0x20, 2, data);
ADAMSTRUTT 23:c1c20aee64a0 117
ADAMSTRUTT 23:c1c20aee64a0 118 return(rVal);
ADAMSTRUTT 23:c1c20aee64a0 119 }
ADAMSTRUTT 23:c1c20aee64a0 120
wm70 18:b79a9a7cf5fb 121 int AX12::isMoving(void) {
wm70 18:b79a9a7cf5fb 122
wm70 18:b79a9a7cf5fb 123 char data[1];
wm70 18:b79a9a7cf5fb 124 read(_ID,AX12_REG_MOVING,1,data);
wm70 18:b79a9a7cf5fb 125 return(data[0]);
wm70 9:87558e7cfecb 126 }
wm70 9:87558e7cfecb 127
wm70 13:be885a516a29 128 int AX12::GetLoad(void){
wm70 14:3be8ca0c066d 129 char data[2];
wm70 14:3be8ca0c066d 130 int ErrorCode = read(_ID, AX12_REG_LOAD, 2, data);
wm70 14:3be8ca0c066d 131 int rVal = data[0] + (data[1] << 8);
wm70 13:be885a516a29 132 return (rVal);
wm70 9:87558e7cfecb 133 }
wm70 9:87558e7cfecb 134
wm70 18:b79a9a7cf5fb 135 int AX12::GetPosition(void){
wm70 18:b79a9a7cf5fb 136
wm70 18:b79a9a7cf5fb 137 char data[2];
wm70 18:b79a9a7cf5fb 138
wm70 18:b79a9a7cf5fb 139 int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
wm70 24:462afd19d12e 140 short position = data[0] + (data[1] << 8);
wm70 24:462afd19d12e 141 float angle = ((position * 300)/1023);
wm70 24:462afd19d12e 142
wm70 24:462afd19d12e 143 return (angle);
wm70 18:b79a9a7cf5fb 144 }
wm70 18:b79a9a7cf5fb 145
wm70 9:87558e7cfecb 146 int AX12::read(int ID, int start, int bytes, char* data){
wm70 9:87558e7cfecb 147
wm70 9:87558e7cfecb 148 char TxBuf[16];
wm70 9:87558e7cfecb 149 char sum = 0;
wm70 9:87558e7cfecb 150 char Status[16];
wm70 9:87558e7cfecb 151
wm70 9:87558e7cfecb 152 TxBuf[0] = 0xff;
wm70 9:87558e7cfecb 153 TxBuf[1] = 0xff;
wm70 9:87558e7cfecb 154 TxBuf[2] = ID;
wm70 9:87558e7cfecb 155 TxBuf[3] = 0x04;
wm70 9:87558e7cfecb 156 TxBuf[4] = 0x02;
wm70 9:87558e7cfecb 157 TxBuf[5] = start;
wm70 9:87558e7cfecb 158 TxBuf[6] = bytes;
wm70 9:87558e7cfecb 159
wm70 9:87558e7cfecb 160 sum+= TxBuf[2];
wm70 9:87558e7cfecb 161 sum+= TxBuf[3];
wm70 9:87558e7cfecb 162 sum+= TxBuf[4];
wm70 9:87558e7cfecb 163 sum+= TxBuf[5];
wm70 9:87558e7cfecb 164 sum+= TxBuf[6];
wm70 9:87558e7cfecb 165 TxBuf[7] = 0xff - sum;
wm70 9:87558e7cfecb 166
wm70 9:87558e7cfecb 167 _dir = TRANSMIT;
wm70 9:87558e7cfecb 168
wm70 9:87558e7cfecb 169 for(int i = 0; i <8; i++){
wm70 9:87558e7cfecb 170 _bus.putc(TxBuf[i]);
wm70 9:87558e7cfecb 171 }
wm70 9:87558e7cfecb 172
wm70 9:87558e7cfecb 173 wait(0.00004);
wm70 9:87558e7cfecb 174 _dir = RECEIVE;
wm70 9:87558e7cfecb 175
wm70 9:87558e7cfecb 176 Status[4] = 0XFE;
wm70 9:87558e7cfecb 177 if(_ID!=0xFE){
wm70 9:87558e7cfecb 178 for(int i = 0; i<(6+bytes); i++){
wm70 9:87558e7cfecb 179 Status[i] = _bus.getc();
wm70 9:87558e7cfecb 180 }
wm70 9:87558e7cfecb 181 for(int i = 0; i< Status[3]-2;i++){
wm70 9:87558e7cfecb 182 data[i] = Status[5+i];
wm70 9:87558e7cfecb 183 }
wm70 9:87558e7cfecb 184 }
wm70 9:87558e7cfecb 185 return(Status[4]);
wm70 9:87558e7cfecb 186 }
wm70 9:87558e7cfecb 187
wm70 9:87558e7cfecb 188 int AX12::write(int ID, int start, int bytes, char* data){
wm70 9:87558e7cfecb 189
wm70 9:87558e7cfecb 190 char TxBuf[16];
wm70 9:87558e7cfecb 191 char sum = 0;
wm70 9:87558e7cfecb 192 char Status[6];
wm70 9:87558e7cfecb 193
wm70 9:87558e7cfecb 194 TxBuf[0] = 0xff;
wm70 9:87558e7cfecb 195 TxBuf[1] = 0xff;
wm70 9:87558e7cfecb 196 TxBuf[2] = ID;
wm70 9:87558e7cfecb 197 TxBuf[3] = 3+bytes;
wm70 9:87558e7cfecb 198 TxBuf[4] = 0x03;
wm70 9:87558e7cfecb 199 TxBuf[5] = start;
wm70 9:87558e7cfecb 200 for(char i = 0; i<bytes; i++){
wm70 9:87558e7cfecb 201 TxBuf[6+i] = data[i];
wm70 9:87558e7cfecb 202 sum+= TxBuf[6+i];
wm70 9:87558e7cfecb 203 }
wm70 9:87558e7cfecb 204
wm70 9:87558e7cfecb 205 sum+= TxBuf[2];
wm70 9:87558e7cfecb 206 sum+= TxBuf[3];
wm70 9:87558e7cfecb 207 sum+= TxBuf[4];
wm70 9:87558e7cfecb 208 sum+= TxBuf[5];
wm70 9:87558e7cfecb 209 TxBuf[6+bytes] = 0xff - sum;
wm70 9:87558e7cfecb 210
wm70 9:87558e7cfecb 211 _dir = TRANSMIT;
wm70 9:87558e7cfecb 212 for(int i = 0; i <(7+bytes); i++){
wm70 9:87558e7cfecb 213 _bus.putc(TxBuf[i]);
wm70 9:87558e7cfecb 214 }
wm70 9:87558e7cfecb 215
wm70 9:87558e7cfecb 216 wait(0.00004);
wm70 9:87558e7cfecb 217 _dir = RECEIVE;
wm70 9:87558e7cfecb 218
wm70 9:87558e7cfecb 219 Status[4] = 0X00;
wm70 9:87558e7cfecb 220 if(_ID!=0xFE){
wm70 9:87558e7cfecb 221 for(int i = 0; i<(6); i++){
wm70 9:87558e7cfecb 222 Status[i] = _bus.getc();
wm70 9:87558e7cfecb 223 }
wm70 9:87558e7cfecb 224 }
wm70 9:87558e7cfecb 225 return(Status[4]);
wm70 9:87558e7cfecb 226 }