Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
AX12.cpp
- Committer:
- wm70
- Date:
- 2016-02-15
- Revision:
- 21:ab49231d1479
- Parent:
- 20:d93f0af76c7a
- Child:
- 23:c1c20aee64a0
File content as of revision 21:ab49231d1479:
#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 flags){ char reg_flag = 0; char data[2]; if (flags == 0x2) { reg_flag = 1; } short goal = (1023*degrees)/300; data[0] = goal & 0xff; data[1] = goal >> 8; int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data); if (flags == 1){ while (isMoving()) {} } return(rVal); } 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::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); int position = data[0] + (data[1] << 8); // int angle = (position * 300)/1023; // (1023 * degrees) / 300 return (position); } 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]); }