ok
Dependencies: mbed
Fork of AX12 by
Diff: AX12.cpp
- Revision:
- 1:93ad80f5fde7
- Parent:
- 0:be51952765ec
- Child:
- 2:5ea99c37a2d7
--- a/AX12.cpp Thu Jun 03 14:22:25 2010 +0000 +++ b/AX12.cpp Wed Mar 30 16:08:02 2011 +0000 @@ -24,7 +24,6 @@ #include "AX12.h" #include "mbed.h" - AX12::AX12(PinName tx, PinName rx, int ID) : _ax12(tx,rx) { @@ -32,14 +31,57 @@ _ID = ID; } -// return 1 is the servo is still in flight -int AX12::isMoving(void) { +// 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) { // set CR + SetCWLimit(0); + SetCCWLimit(0); + SetCRSpeed(0.0); + } else { + SetCWLimit(0); + SetCCWLimit(300); + SetCRSpeed(0.0); + } + return(0); +} + + +// if flag[0] is set, were blocking +// if flag[1] is set, we're registering +// they are mutually exclusive operations +int AX12::SetGoal(int degrees, int flags) { - char data[1]; - read(_ID,AX12_REG_MOVING,1,data); - return(data[0]); + char reg_flag = 0; + char data[2]; + + // set the flag is only the register bit is set in the flag + if (flags == 0x2) { + reg_flag = 1; + } + + // 1023 / 300 * degrees + short goal = (1023 * degrees) / 300; + if (AX12_DEBUG) { + printf("SetGoal to 0x%x\n",goal); + } + + 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, AX12_REG_GOAL_POSITION, 2, data, reg_flag); + + if (flags == 1) { + // block until it comes to a halt + while (isMoving()) {} + } + return(rVal); } + // Set continuous rotation speed from -1 to 1 int AX12::SetCRSpeed(float speed) { @@ -63,44 +105,66 @@ return(rVal); } -// Set the mode of the servo -// 0 = Positional (0-300 degrees) -// 1 = Rotational -1 to 1 speed -int AX12::SetMode(int mode) { + +int AX12::SetCWLimit (int degrees) { + + char data[2]; + + // 1023 / 300 * degrees + short limit = (1023 * degrees) / 300; - if (mode == 1) { // set CR - SetCWLimit(0); - SetCCWLimit(0); - SetCRSpeed(0.0); - } else { - SetCWLimit(0); - SetCCWLimit(300); - SetCRSpeed(0.0); + if (AX12_DEBUG) { + printf("SetCWLimit to 0x%x\n",limit); } - return(0); + + 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)); + } -float AX12::GetTemp (void) { +int AX12::SetCCWLimit (int degrees) { + + char data[2]; + + // 1023 / 300 * degrees + short limit = (1023 * degrees) / 300; if (AX12_DEBUG) { - printf("\nGetTemp(%d)",_ID); + printf("SetCCWLimit to 0x%x\n",limit); } - char data[1]; - int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data); - float temp = data[0]; - return(temp); + + 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)); } -float AX12::GetVolts (void) { - if (AX12_DEBUG) { - printf("\nGetVolts(%d)",_ID); - } + +int AX12::SetID (int CurrentID, int NewID) { + char data[1]; - int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data); - float volts = data[0]/10.0; - return(volts); + data[0] = NewID; + if (AX12_DEBUG) { + printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); + } + return (write(CurrentID, AX12_REG_ID, 1, data)); + } + +// return 1 is the servo is still in flight +int AX12::isMoving(void) { + + char data[1]; + read(_ID,AX12_REG_MOVING,1,data); + return(data[0]); +} + + void AX12::trigger(void) { char TxBuf[16]; @@ -150,47 +214,12 @@ for (int i = 0; i < 6 ; i++) { _ax12.putc(TxBuf[i]); } - + // This is a broadcast packet, so there will be no reply return; } -int AX12::SetGoal(int degrees, int flags) { - -// if flag[0] is set, were blocking -// if flag[1] is set, we're registering -// they are mutually exclusive operations - - char reg_flag = 0; - - char data[2]; - - // set the flag is only the register bit is set in the flag - if (flags == 0x2) { - reg_flag = 1; - } - - // 1023 / 300 * degrees - short goal = (1023 * degrees) / 300; - if (AX12_DEBUG) { - printf("SetGoal to 0x%x\n",goal); - } - - 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, AX12_REG_GOAL_POSITION, 2, data, reg_flag); - - if (flags == 1) { - // block until it comes to a halt - while (isMoving()) {} - } - - return(rVal); - -} float AX12::GetPosition(void) { @@ -207,52 +236,27 @@ return (angle); } -int AX12::SetCWLimit (int degrees) { - char data[2]; - - // 1023 / 300 * degrees - short limit = (1023 * degrees) / 300; +float AX12::GetTemp (void) { if (AX12_DEBUG) { - printf("SetCWLimit to 0x%x\n",limit); + printf("\nGetTemp(%d)",_ID); } - - 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)); - + char data[1]; + int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data); + float temp = data[0]; + return(temp); } -int AX12::SetCCWLimit (int degrees) { - char data[2]; - - // 1023 / 300 * degrees - short limit = (1023 * degrees) / 300; - +float AX12::GetVolts (void) { if (AX12_DEBUG) { - printf("SetCCWLimit to 0x%x\n",limit); + printf("\nGetVolts(%d)",_ID); } - - 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::SetID (int CurrentID, int NewID) { - char data[1]; - data[0] = NewID; - if (AX12_DEBUG) { - printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); - } - return (write(CurrentID, AX12_REG_ID, 1, data)); - + int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data); + float volts = data[0]/10.0; + return(volts); } @@ -359,11 +363,9 @@ return(Status[4]); } + int AX12:: write(int ID, int start, int bytes, char* data, int flag) { - -// number of bytes in packet // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum -// 7 + bytes char TxBuf[16]; char sum = 0;