Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
AX12.cpp
- Committer:
- wm70
- Date:
- 2016-02-03
- Revision:
- 14:3be8ca0c066d
- Parent:
- 13:be885a516a29
- Child:
- 18:b79a9a7cf5fb
File content as of revision 14:3be8ca0c066d:
#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){ char data[4]; short int goal = (1023*degrees)/300; data[0] = goal & 0xff; data[1] = goal >> 8; data[2] = 0xf4; data[3] = 0x01; int rVal = write(_ID, AX12_REG_GOAL_POSITION, 4, data); return(rVal); } 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::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]); }