Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
AX12.cpp@20:d93f0af76c7a, 2016-02-15 (annotated)
- Committer:
- wm70
- Date:
- Mon Feb 15 15:22:58 2016 +0000
- Revision:
- 20:d93f0af76c7a
- Parent:
- 19:bf64727d5b0e
- Child:
- 21:ab49231d1479
Torque - yes ; position - no
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 | |
wm70 | 18:b79a9a7cf5fb | 13 | int AX12::SetGoal(int degrees, int flags){ |
wm70 | 18:b79a9a7cf5fb | 14 | |
wm70 | 18:b79a9a7cf5fb | 15 | char reg_flag = 0; |
wm70 | 18:b79a9a7cf5fb | 16 | char data[2]; |
wm70 | 18:b79a9a7cf5fb | 17 | |
wm70 | 18:b79a9a7cf5fb | 18 | if (flags == 0x2) { |
wm70 | 18:b79a9a7cf5fb | 19 | reg_flag = 1; |
wm70 | 18:b79a9a7cf5fb | 20 | } |
wm70 | 18:b79a9a7cf5fb | 21 | |
wm70 | 18:b79a9a7cf5fb | 22 | short goal = (1023*degrees)/300; |
wm70 | 9:87558e7cfecb | 23 | data[0] = goal & 0xff; |
wm70 | 9:87558e7cfecb | 24 | data[1] = goal >> 8; |
wm70 | 18:b79a9a7cf5fb | 25 | |
wm70 | 18:b79a9a7cf5fb | 26 | int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data); |
wm70 | 18:b79a9a7cf5fb | 27 | |
wm70 | 18:b79a9a7cf5fb | 28 | if (flags == 1){ |
wm70 | 18:b79a9a7cf5fb | 29 | while (isMoving()) {} |
wm70 | 18:b79a9a7cf5fb | 30 | } |
wm70 | 18:b79a9a7cf5fb | 31 | return(rVal); |
wm70 | 18:b79a9a7cf5fb | 32 | } |
wm70 | 9:87558e7cfecb | 33 | |
wm70 | 18:b79a9a7cf5fb | 34 | int AX12::SetCWLimit (int degrees) { |
wm70 | 18:b79a9a7cf5fb | 35 | |
wm70 | 18:b79a9a7cf5fb | 36 | char data[2]; |
wm70 | 18:b79a9a7cf5fb | 37 | // 1023 / 300 * degrees |
wm70 | 18:b79a9a7cf5fb | 38 | short limit = (1023 * degrees) / 300; |
wm70 | 18:b79a9a7cf5fb | 39 | |
wm70 | 18:b79a9a7cf5fb | 40 | data[0] = limit & 0xff; // bottom 8 bits |
wm70 | 18:b79a9a7cf5fb | 41 | data[1] = limit >> 8; // top 8 bits |
wm70 | 18:b79a9a7cf5fb | 42 | |
wm70 | 18:b79a9a7cf5fb | 43 | // write the packet, return the error code |
wm70 | 18:b79a9a7cf5fb | 44 | return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); |
wm70 | 18:b79a9a7cf5fb | 45 | } |
wm70 | 18:b79a9a7cf5fb | 46 | |
wm70 | 18:b79a9a7cf5fb | 47 | int AX12::SetCCWLimit (int degrees) { |
wm70 | 18:b79a9a7cf5fb | 48 | |
wm70 | 18:b79a9a7cf5fb | 49 | char data[2]; |
wm70 | 18:b79a9a7cf5fb | 50 | |
wm70 | 18:b79a9a7cf5fb | 51 | // 1023 / 300 * degrees |
wm70 | 18:b79a9a7cf5fb | 52 | short limit = (1023 * degrees) / 300; |
wm70 | 18:b79a9a7cf5fb | 53 | |
wm70 | 18:b79a9a7cf5fb | 54 | data[0] = limit & 0xff; // bottom 8 bits |
wm70 | 18:b79a9a7cf5fb | 55 | data[1] = limit >> 8; // top 8 bits |
wm70 | 18:b79a9a7cf5fb | 56 | |
wm70 | 18:b79a9a7cf5fb | 57 | // write the packet, return the error code |
wm70 | 18:b79a9a7cf5fb | 58 | return (write(_ID, AX12_REG_CCW_LIMIT, 2, data)); |
wm70 | 18:b79a9a7cf5fb | 59 | } |
wm70 | 18:b79a9a7cf5fb | 60 | |
wm70 | 18:b79a9a7cf5fb | 61 | int AX12::isMoving(void) { |
wm70 | 18:b79a9a7cf5fb | 62 | |
wm70 | 18:b79a9a7cf5fb | 63 | char data[1]; |
wm70 | 18:b79a9a7cf5fb | 64 | read(_ID,AX12_REG_MOVING,1,data); |
wm70 | 18:b79a9a7cf5fb | 65 | return(data[0]); |
wm70 | 9:87558e7cfecb | 66 | } |
wm70 | 9:87558e7cfecb | 67 | |
wm70 | 13:be885a516a29 | 68 | int AX12::GetLoad(void){ |
wm70 | 14:3be8ca0c066d | 69 | char data[2]; |
wm70 | 14:3be8ca0c066d | 70 | int ErrorCode = read(_ID, AX12_REG_LOAD, 2, data); |
wm70 | 14:3be8ca0c066d | 71 | int rVal = data[0] + (data[1] << 8); |
wm70 | 13:be885a516a29 | 72 | return (rVal); |
wm70 | 9:87558e7cfecb | 73 | } |
wm70 | 9:87558e7cfecb | 74 | |
wm70 | 18:b79a9a7cf5fb | 75 | int AX12::GetPosition(void){ |
wm70 | 18:b79a9a7cf5fb | 76 | |
wm70 | 18:b79a9a7cf5fb | 77 | char data[2]; |
wm70 | 18:b79a9a7cf5fb | 78 | |
wm70 | 18:b79a9a7cf5fb | 79 | int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data); |
wm70 | 19:bf64727d5b0e | 80 | int position = data[0] + (data[1] << 8); |
wm70 | 20:d93f0af76c7a | 81 | int angle = (position * 300)/1023; |
wm70 | 18:b79a9a7cf5fb | 82 | return (angle); |
wm70 | 18:b79a9a7cf5fb | 83 | } |
wm70 | 18:b79a9a7cf5fb | 84 | |
wm70 | 9:87558e7cfecb | 85 | int AX12::read(int ID, int start, int bytes, char* data){ |
wm70 | 9:87558e7cfecb | 86 | |
wm70 | 9:87558e7cfecb | 87 | char TxBuf[16]; |
wm70 | 9:87558e7cfecb | 88 | char sum = 0; |
wm70 | 9:87558e7cfecb | 89 | char Status[16]; |
wm70 | 9:87558e7cfecb | 90 | |
wm70 | 9:87558e7cfecb | 91 | TxBuf[0] = 0xff; |
wm70 | 9:87558e7cfecb | 92 | TxBuf[1] = 0xff; |
wm70 | 9:87558e7cfecb | 93 | TxBuf[2] = ID; |
wm70 | 9:87558e7cfecb | 94 | TxBuf[3] = 0x04; |
wm70 | 9:87558e7cfecb | 95 | TxBuf[4] = 0x02; |
wm70 | 9:87558e7cfecb | 96 | TxBuf[5] = start; |
wm70 | 9:87558e7cfecb | 97 | TxBuf[6] = bytes; |
wm70 | 9:87558e7cfecb | 98 | |
wm70 | 9:87558e7cfecb | 99 | sum+= TxBuf[2]; |
wm70 | 9:87558e7cfecb | 100 | sum+= TxBuf[3]; |
wm70 | 9:87558e7cfecb | 101 | sum+= TxBuf[4]; |
wm70 | 9:87558e7cfecb | 102 | sum+= TxBuf[5]; |
wm70 | 9:87558e7cfecb | 103 | sum+= TxBuf[6]; |
wm70 | 9:87558e7cfecb | 104 | TxBuf[7] = 0xff - sum; |
wm70 | 9:87558e7cfecb | 105 | |
wm70 | 9:87558e7cfecb | 106 | _dir = TRANSMIT; |
wm70 | 9:87558e7cfecb | 107 | |
wm70 | 9:87558e7cfecb | 108 | for(int i = 0; i <8; i++){ |
wm70 | 9:87558e7cfecb | 109 | _bus.putc(TxBuf[i]); |
wm70 | 9:87558e7cfecb | 110 | } |
wm70 | 9:87558e7cfecb | 111 | |
wm70 | 9:87558e7cfecb | 112 | wait(0.00004); |
wm70 | 9:87558e7cfecb | 113 | _dir = RECEIVE; |
wm70 | 9:87558e7cfecb | 114 | |
wm70 | 9:87558e7cfecb | 115 | Status[4] = 0XFE; |
wm70 | 9:87558e7cfecb | 116 | if(_ID!=0xFE){ |
wm70 | 9:87558e7cfecb | 117 | for(int i = 0; i<(6+bytes); i++){ |
wm70 | 9:87558e7cfecb | 118 | Status[i] = _bus.getc(); |
wm70 | 9:87558e7cfecb | 119 | } |
wm70 | 9:87558e7cfecb | 120 | for(int i = 0; i< Status[3]-2;i++){ |
wm70 | 9:87558e7cfecb | 121 | data[i] = Status[5+i]; |
wm70 | 9:87558e7cfecb | 122 | } |
wm70 | 9:87558e7cfecb | 123 | } |
wm70 | 9:87558e7cfecb | 124 | return(Status[4]); |
wm70 | 9:87558e7cfecb | 125 | } |
wm70 | 9:87558e7cfecb | 126 | |
wm70 | 9:87558e7cfecb | 127 | int AX12::write(int ID, int start, int bytes, char* data){ |
wm70 | 9:87558e7cfecb | 128 | |
wm70 | 9:87558e7cfecb | 129 | char TxBuf[16]; |
wm70 | 9:87558e7cfecb | 130 | char sum = 0; |
wm70 | 9:87558e7cfecb | 131 | char Status[6]; |
wm70 | 9:87558e7cfecb | 132 | |
wm70 | 9:87558e7cfecb | 133 | TxBuf[0] = 0xff; |
wm70 | 9:87558e7cfecb | 134 | TxBuf[1] = 0xff; |
wm70 | 9:87558e7cfecb | 135 | TxBuf[2] = ID; |
wm70 | 9:87558e7cfecb | 136 | TxBuf[3] = 3+bytes; |
wm70 | 9:87558e7cfecb | 137 | TxBuf[4] = 0x03; |
wm70 | 9:87558e7cfecb | 138 | TxBuf[5] = start; |
wm70 | 9:87558e7cfecb | 139 | for(char i = 0; i<bytes; i++){ |
wm70 | 9:87558e7cfecb | 140 | TxBuf[6+i] = data[i]; |
wm70 | 9:87558e7cfecb | 141 | sum+= TxBuf[6+i]; |
wm70 | 9:87558e7cfecb | 142 | } |
wm70 | 9:87558e7cfecb | 143 | |
wm70 | 9:87558e7cfecb | 144 | sum+= TxBuf[2]; |
wm70 | 9:87558e7cfecb | 145 | sum+= TxBuf[3]; |
wm70 | 9:87558e7cfecb | 146 | sum+= TxBuf[4]; |
wm70 | 9:87558e7cfecb | 147 | sum+= TxBuf[5]; |
wm70 | 9:87558e7cfecb | 148 | TxBuf[6+bytes] = 0xff - sum; |
wm70 | 9:87558e7cfecb | 149 | |
wm70 | 9:87558e7cfecb | 150 | _dir = TRANSMIT; |
wm70 | 9:87558e7cfecb | 151 | for(int i = 0; i <(7+bytes); i++){ |
wm70 | 9:87558e7cfecb | 152 | _bus.putc(TxBuf[i]); |
wm70 | 9:87558e7cfecb | 153 | } |
wm70 | 9:87558e7cfecb | 154 | |
wm70 | 9:87558e7cfecb | 155 | wait(0.00004); |
wm70 | 9:87558e7cfecb | 156 | _dir = RECEIVE; |
wm70 | 9:87558e7cfecb | 157 | |
wm70 | 9:87558e7cfecb | 158 | Status[4] = 0X00; |
wm70 | 9:87558e7cfecb | 159 | if(_ID!=0xFE){ |
wm70 | 9:87558e7cfecb | 160 | for(int i = 0; i<(6); i++){ |
wm70 | 9:87558e7cfecb | 161 | Status[i] = _bus.getc(); |
wm70 | 9:87558e7cfecb | 162 | } |
wm70 | 9:87558e7cfecb | 163 | } |
wm70 | 9:87558e7cfecb | 164 | return(Status[4]); |
wm70 | 9:87558e7cfecb | 165 | } |