Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
Diff: AX12.cpp
- Revision:
- 23:c1c20aee64a0
- Parent:
- 21:ab49231d1479
- Child:
- 24:462afd19d12e
--- a/AX12.cpp Mon Feb 15 15:55:27 2016 +0000 +++ b/AX12.cpp Wed Feb 17 11:55:04 2016 +0000 @@ -10,25 +10,56 @@ _dir = TRANSMIT; } -int AX12::SetGoal(int degrees, int flags){ - - char reg_flag = 0; - char data[2]; - - if (flags == 0x2) { - reg_flag = 1; - } +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, 2, data); + + 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 -if (flags == 1){ - while (isMoving()) {} +int AX12::SetMode(int mode) { + + if (mode == 1) { + + SetCWLimit(0); + SetCCWLimit(0); + SetCRSpeed(0.0); + } else { + SetCWLimit(70); + SetCCWLimit(270); + SetCRSpeed(0.0); } - return(rVal); + return(0); +} + +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 + } + + // write the packet, return the error code + return (write(_ID, AX12_REG_TORQUE_LIMIT, 2, data)); } int AX12::SetCWLimit (int degrees) { @@ -58,6 +89,28 @@ 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];