Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
Diff: AX12.cpp
- Revision:
- 24:462afd19d12e
- Parent:
- 23:c1c20aee64a0
- Child:
- 25:d345ef4cb2b5
diff -r c1c20aee64a0 -r 462afd19d12e AX12.cpp --- a/AX12.cpp Wed Feb 17 11:55:04 2016 +0000 +++ b/AX12.cpp Thu Mar 03 12:07:25 2016 +0000 @@ -48,18 +48,25 @@ int AX12::SetTorqueLimit (int torque) { char data[2]; - - if(torque ==1){ - data[0] = 0xff; // bottom 8 bits - data[1] = 0xff; // top 8 bits - }else{ - - data[0] = 0x00; // bottom 8 bits - data[1] = 0x00; // top 8 bits - } + 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) { @@ -130,10 +137,10 @@ 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); + 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){