Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
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?
User | Revision | Line number | New 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 | } |