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@19:bf64727d5b0e, 2016-02-11 (annotated)

Committer:
wm70
Date:
Thu Feb 11 16:37:02 2016 +0000
Revision:
19:bf64727d5b0e
Parent:
18:b79a9a7cf5fb
Child:
20:d93f0af76c7a
set limits; Position ; Torque

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
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 18:b79a9a7cf5fb 81 int angle = (position * 300)/1024;
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 }