Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
AX12.cpp
- Committer:
- wm70
- Date:
- 2016-03-03
- Revision:
- 25:d345ef4cb2b5
- Parent:
- 24:462afd19d12e
- Child:
- 27:d032d8fd4b45
File content as of revision 25:d345ef4cb2b5:
#include "AX12.h" #include "mbed.h" #include "string" AX12::AX12(Serial& bus, Serial& PCbus, PinName dir, int ID) : _bus(bus), _PCbus(PCbus), _dir(dir){ _bus.baud(1000000); _ID = ID; _dir = TRANSMIT; } int AX12::SetGoal(int degrees, int speed){ short goal = (1023*degrees)/300; char data[4]; data[0] = goal & 0xff; data[1] = goal >> 8; data[2] = 0x00; data[3] = 0x00; int rVal = write(_ID, AX12_REG_GOAL_POSITION, 4, data); return (rVal); } // Set the mode of the servo // 0 = Positional (0-300 degrees) // 1 = Rotational -1 to 1 speed int AX12::SetMode(int mode) { if (mode == 1) { SetCWLimit(0); SetCCWLimit(0); SetCRSpeed(0.0); } else { SetCWLimit(70); SetCCWLimit(270); SetCRSpeed(0.0); } return(0); } int AX12::SetTorqueLimit (int torque) { char data[2]; int set = (torque*1023)/10; data[0] = set && 0xff; // bottom 8 bits data[1] = set >> 8; // top 8 bits // write the packet, return the error code return (write(_ID, AX12_REG_TORQUE_LIMIT, 2, data)); } int AX12::TorqueEnable (int enable) { char data[1]; data[0] = enable; return (write(_ID, AX12_REG_TORQUE_ENABLE, 1, data)); } int AX12::SetCWLimit (int degrees) { char data[2]; // 1023 / 300 * degrees short limit = (1023 * degrees) / 300; data[0] = limit & 0xff; // bottom 8 bits data[1] = limit >> 8; // top 8 bits // write the packet, return the error code return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); } int AX12::SetCCWLimit (int degrees) { char data[2]; // 1023 / 300 * degrees short limit = (1023 * degrees) / 300; data[0] = limit & 0xff; // bottom 8 bits data[1] = limit >> 8; // top 8 bits // write the packet, return the error code return (write(_ID, AX12_REG_CCW_LIMIT, 2, data)); } int AX12::SetCRSpeed(float speed) { // bit 10 = direction, 0 = CCW, 1=CW // bits 9-0 = Speed char data[2]; int goal = (0x3ff * abs(speed)); // Set direction CW if we have a negative speed if (speed < 0) { goal |= (0x1 << 10); } data[0] = goal & 0xff; // bottom 8 bits data[1] = goal >> 8; // top 8 bits // write the packet, return the error code int rVal = write(_ID, 0x20, 2, data); return(rVal); } int AX12::isMoving(void) { char data[1]; read(_ID,AX12_REG_MOVING,1,data); return(data[0]); } int AX12::GetLoad(void){ char data[2]; int ErrorCode = read(_ID, AX12_REG_LOAD, 2, data); int rVal = data[0] + (data[1] << 8); return (rVal); } int AX12::GetPosition(void){ char data[2]; int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data); short position = data[0] + (data[1] << 8); float angle = ((position * 300)/1023); return (angle); } int AX12::read(int ID, int start, int bytes, char* data){ char TxBuf[16]; char sum = 0; char Status[16]; TxBuf[0] = 0xff; TxBuf[1] = 0xff; TxBuf[2] = ID; TxBuf[3] = 0x04; TxBuf[4] = 0x02; TxBuf[5] = start; TxBuf[6] = bytes; sum+= TxBuf[2]; sum+= TxBuf[3]; sum+= TxBuf[4]; sum+= TxBuf[5]; sum+= TxBuf[6]; TxBuf[7] = 0xff - sum; _dir = TRANSMIT; for(int i = 0; i <8; i++){ _bus.putc(TxBuf[i]); } wait(0.00004); _dir = RECEIVE; Status[4] = 0XFE; if(_ID!=0xFE){ for(int i = 0; i<(6+bytes); i++){ Status[i] = _bus.getc(); } for(int i = 0; i< Status[3]-2;i++){ data[i] = Status[5+i]; } } return(Status[4]); } int AX12::write(int ID, int start, int bytes, char* data){ char TxBuf[16]; char sum = 0; char Status[6]; TxBuf[0] = 0xff; TxBuf[1] = 0xff; TxBuf[2] = ID; TxBuf[3] = 3+bytes; TxBuf[4] = 0x03; TxBuf[5] = start; for(char i = 0; i<bytes; i++){ TxBuf[6+i] = data[i]; sum+= TxBuf[6+i]; } sum+= TxBuf[2]; sum+= TxBuf[3]; sum+= TxBuf[4]; sum+= TxBuf[5]; TxBuf[6+bytes] = 0xff - sum; _dir = TRANSMIT; for(int i = 0; i <(7+bytes); i++){ _bus.putc(TxBuf[i]); } wait(0.00004); _dir = RECEIVE; Status[4] = 0X00; if(_ID!=0xFE){ for(int i = 0; i<(6); i++){ Status[i] = _bus.getc(); } } return(Status[4]); }