void ServoDriver::OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1)
#endif
{
word wCoxaSDV; // Coxa value in servo driver units
word wFemurSDV; //
word wTibiaSDV; //
#ifdef c4DOF
word wTarsSDV; //
#endif
// The Main code now takes care of the inversion before calling.
wCoxaSDV = (((long)(sCoxaAngle1))* cPwmMult) / cPwmDiv +cPFConst;
wFemurSDV = (((long)((long)(sFemurAngle1))* cPwmMult) / cPwmDiv +cPFConst);
wTibiaSDV = (((long)(sTibiaAngle1))* cPwmMult) / cPwmDiv +cPFConst;
#ifdef c4DOF
wTarsSDV = (((long)(sTarsAngle1))* cPwmMult) / cPwmDiv +cPFConst;
#endif
if (ServosEnabled) {
if (g_fAXSpeedControl) {
#ifdef USE_AX12_SPEED_CONTROL
// Save away the new positions...
g_awGoalAXPos[FIRSTCOXAPIN+LegIndex] = wCoxaSDV; // What order should we store these values?
g_awGoalAXPos[FIRSTFEMURPIN+LegIndex] = wFemurSDV;
g_awGoalAXPos[FIRSTTIBIAPIN+LegIndex] = wTibiaSDV;
#ifdef c4DOF
g_awGoalAXTarsPos[FIRSTTARSPIN+LegIndex] = wTarsSDV;
#endif
#endif
}
else {
// With new library we set by Index. Note Index defaults to servoID - 1
bioloid.setNextPoseByIndex(FIRSTCOXAPIN+LegIndex, wCoxaSDV);
bioloid.setNextPoseByIndex(FIRSTFEMURPIN+LegIndex, wFemurSDV);
bioloid.setNextPoseByIndex(FIRSTTIBIAPIN+LegIndex, wTibiaSDV);
#ifdef c4DOF
if ((byte)(cTarsLength[LegIndex])) // We allow mix of 3 and 4 DOF legs...
bioloid.setNextPoseByIndex(FIRSTTARSPIN+LegIndex, wTarsSDV);
#endif
}
}
#ifdef DEBUG_SERVOS
if (g_fDebugOutput) {
DBGSerial.print(LegIndex, DEC);
DBGSerial.print("(");
DBGSerial.print(sCoxaAngle1, DEC);
DBGSerial.print("=");
DBGSerial.print(wCoxaSDV, DEC);
DBGSerial.print("),(");
DBGSerial.print(sFemurAngle1, DEC);
DBGSerial.print("=");
DBGSerial.print(wFemurSDV, DEC);
DBGSerial.print("),(");
DBGSerial.print("(");
DBGSerial.print(sTibiaAngle1, DEC);
DBGSerial.print("=");
DBGSerial.print(wTibiaSDV, DEC);
DBGSerial.print(") :");
}
#endif
g_InputController.AllowControllerInterrupts(true); // Ok for hserial again...
}
//===================================================================================================
// Setup
//====================================================================================================
void setup() {
// Lets initialize the Commander
Serial.begin();
if (!command.begin("/dev/ttyXBEE", B38400))
{
printf("Commander Begin failed\n");
return;
}
// Next initialize the Bioloid
bioloid.poseSize = CNT_SERVOS;
// Read in the current positions...
printf("Before readPose\n");
bioloid.readPose();
printf("After readPose\n");
for (int i=1; i <= CNT_SERVOS; i++) {
Serial.println(dxl_read_word(i, AX_PRESENT_POSITION_L), DEC);
}
// Start off to put arm to sleep...
Serial.println("Kurt's Arm");
PutArmToSleep();
MSound(3, 60, 2000, 80, 2250, 100, 2500);
}
void ServoDriver::OutputServoInfoForLeg(int LegIndex, float CoxaAngle, float FemurAngle, float TibiaAngle)
#endif
{
word wCoxaSDV; // Coxa value in servo driver units
word wFemurSDV; //
word wTibiaSDV; //
#ifdef c4DOF
word wTarsSDV; //
#endif
// The Main code now takes care of the inversion before calling.
wCoxaSDV = CoxaAngle * SERVO_TIC_PER_DEG + SERVO_CENTER_VALUE;
wFemurSDV = FemurAngle * SERVO_TIC_PER_DEG + SERVO_CENTER_VALUE;
wTibiaSDV = TibiaAngle * SERVO_TIC_PER_DEG + SERVO_CENTER_VALUE;
#ifdef c4DOF
wTarsSDV = TarsAngle * SERVO_TIC_PER_DEG + SERVO_CENTER_VALUE;
#endif
if (ServosEnabled) {
// With new library we set by Index. Note Index defaults to servoID - 1
bioloid.setNextPoseByIndex(pgm_read_byte(&cPinTable[FIRSTCOXAPIN+LegIndex])-1, wCoxaSDV);
bioloid.setNextPoseByIndex(pgm_read_byte(&cPinTable[FIRSTFEMURPIN+LegIndex])-1, wFemurSDV);
bioloid.setNextPoseByIndex(pgm_read_byte(&cPinTable[FIRSTTIBIAPIN+LegIndex])-1, wTibiaSDV);
#ifdef c4DOF
if ((byte)pgm_read_byte(&cTarsLength[LegIndex])) // We allow mix of 3 and 4 DOF legs...
bioloid.setNextPoseByIndex(pgm_read_byte(&cPinTable[FIRSTTARSPIN+LegIndex])-1, wTarsSDV);
#endif
}
#ifdef DEBUG_SERVOS
if (g_fDebugOutput) {
DBGSerial.print(LegIndex, DEC);
DBGSerial.print("(");
DBGSerial.print(sCoxaAngle1, DEC);
DBGSerial.print("=");
DBGSerial.print(wCoxaSDV, DEC);
DBGSerial.print("),(");
DBGSerial.print(sFemurAngle1, DEC);
DBGSerial.print("=");
DBGSerial.print(wFemurSDV, DEC);
DBGSerial.print("),(");
DBGSerial.print("(");
DBGSerial.print(sTibiaAngle1, DEC);
DBGSerial.print("=");
DBGSerial.print(wTibiaSDV, DEC);
DBGSerial.print(") :");
}
#endif
g_InputController.AllowControllerInterrupts(true); // Ok for hserial again...
}
//--------------------------------------------------------------------
//[MakeSureServosAreOn] Function that is called to handle when you are
// transistioning from servos all off to being on. May need to read
// in the current pose...
//--------------------------------------------------------------------
void MakeSureServosAreOn(void)
{
if (ServosEnabled) {
if (!g_fServosFree)
return; // we are not free
g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess...
bioloid.readPose();
SetRegOnAllServos(AX_TORQUE_ENABLE, 1);
g_InputController.AllowControllerInterrupts(true);
g_fServosFree = false;
}
}
//--------------------------------------------------------------------
// Cleanup
//--------------------------------------------------------------------
void ServoDriver::Cleanup(void) {
// Do any cleanup that the driver may need.
printf("ServoDriver::Cleanup\n\r");
#ifdef USB2AX_REG_VOLTAGE
ax12SetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, 0); // Turn off the voltage testing...
#endif
// Turn off all of the servo LEDS... Maybe use broadcast?
for (int iServo=0; iServo < NUMSERVOS; iServo++) {
dxl_write_byte((cPinTable[iServo]), AX_LED, 0);
dxl_get_result(); // don't care for now
}
bioloid.end(); // tell the driver to abort
}
//--------------------------------------------------------------------
//Init
//--------------------------------------------------------------------
void ServoDriver::Init(void) {
// First lets get the actual servo positions for all of our servos...
g_fServosFree = true;
bioloid.poseSize(NUMSERVOS); // Method in this version
bioloid.readPose();
ax12SetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, cPinTable[FIRSTFEMURPIN]);
g_fAXSpeedControl = false;
#ifdef OPT_GPPLAYER
_fGPEnabled = true; // assume we support it.
#endif
// Currently have Turret pins not necessarily same as numerical order so
// Maybe should do for all pins and then set the positions by index instead
// of having it do a simple search on each pin...
#ifdef cTurretRotPin
bioloid.setId(FIRSTTURRETPIN, cTurretRotPin);
bioloid.setId(FIRSTTURRETPIN+1, cTurretTiltPin);
#endif
}
void ServoDriver::OutputServoInfoForTurret(float RotateAngle, float TiltAngle)
{
word wRotateSDV;
word wTiltSDV; //
// The Main code now takes care of the inversion before calling.
wRotateSDV = RotateAngle * SERVO_TIC_PER_DEG + SERVO_CENTER_VALUE;
wTiltSDV = TiltAngle * SERVO_TIC_PER_DEG + SERVO_CENTER_VALUE;
if (ServosEnabled) {
if (g_fAXSpeedControl) {
#ifdef USE_AX12_SPEED_CONTROL
// Save away the new positions...
g_awGoalAXPos[FIRSTTURRETPIN] = wRotateSDV; // What order should we store these values?
g_awGoalAXPos[FIRSTTURRETPIN+1] = wTiltSDV;
#endif
}
else {
bioloid.setNextPoseByIndex(FIRSTTURRETPIN, wRotateSDV);
bioloid.setNextPoseByIndex(FIRSTTURRETPIN, wRotateSDV);
bioloid.setNextPoseByIndex(FIRSTTURRETPIN+1, wTiltSDV);
}
}
#ifdef DEBUG_SERVOS
if (g_fDebugOutput) {
DBGSerial.print("(");
DBGSerial.print(sRotateAngle1, DEC);
DBGSerial.print("=");
DBGSerial.print(wRotateSDV, DEC);
DBGSerial.print("),(");
DBGSerial.print(sTiltAngle1, DEC);
DBGSerial.print("=");
DBGSerial.print(wTiltSDV, DEC);
DBGSerial.print(") :");
}
#endif
}
//--------------------------------------------------------------------
//[CommitServoDriver Updates the positions of the servos - This outputs
// as much of the command as we can without committing it. This
// allows us to once the previous update was completed to quickly
// get the next command to start
//--------------------------------------------------------------------
bool ServoDriver::CommitServoDriver(word wMoveTime)
{
#ifdef cSSC_BINARYMODE
byte abOut[3];
#endif
g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess...
if (ServosEnabled) {
bioloid.interpolateSetup(wMoveTime);
}
#ifdef DEBUG_SERVOS
if (g_fDebugOutput)
DBGSerial.println(wMoveTime, DEC);
#endif
g_InputController.AllowControllerInterrupts(true);
return true;
}
//--------------------------------------------------------------------
//[MakeSureServosAreOn] Function that is called to handle when you are
// transistioning from servos all off to being on. May need to read
// in the current pose...
//--------------------------------------------------------------------
void MakeSureServosAreOn(void)
{
if (ServosEnabled) {
if (!g_fServosFree)
return; // we are not free
g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess...
bioloid.readPose();
SetRegOnAllServos(AX_TORQUE_ENABLE, 1);
#if 0
for (byte i = 0; i < NUMSERVOS; i++) {
TorqueOn(pgm_read_byte(&cPinTable[i]));
}
#endif
g_InputController.AllowControllerInterrupts(true);
g_fServosFree = false;
}
}
//===================================================================================================
// MoveArmTo
//===================================================================================================
void MoveArmTo(int sBase, int sShoulder, int sElbow, int sWrist, int sWristRot, int sGrip, int wTime, boolean fWait) {
int sMaxDelta;
int sDelta;
// First make sure servos are not free...
if (g_fServosFree) {
g_fServosFree = false;
for(uint8_t i=1; i <= CNT_SERVOS; i++) {
TorqueOn(i);
}
bioloid.readPose();
}
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.print("[");
Serial.print(sBase, DEC);
Serial.print(" ");
Serial.print(sShoulder, DEC);
Serial.print(" ");
Serial.print(sElbow, DEC);
Serial.print(" ");
Serial.print(sWrist, DEC);
Serial.print(" ");
Serial.print(sWristRot, DEC);
Serial.print(" ");
Serial.print(sGrip, DEC);
Serial.println("]");
}
#endif
// Make sure the previous movement completed.
// Need to do it before setNextPos calls as this
// is used in the interpolating code...
while (bioloid.interpolating() > 0) {
bioloid.interpolateStep();
delay(3);
}
// Also lets limit how fast the servos will move as to not get whiplash.
bioloid.setNextPoseByIndex(SID_BASE, sBase);
sMaxDelta = abs(bioloid.getNextPoseByIndex(SID_RSHOULDER) - sShoulder);
bioloid.setNextPoseByIndex(SID_RSHOULDER, sShoulder);
bioloid.setNextPoseByIndex(SID_LSHOULDER, 1024-sShoulder);
sDelta = abs(bioloid.getNextPoseByIndex(SID_RELBOW) - sElbow);
if (sDelta > sMaxDelta)
sMaxDelta = sDelta;
bioloid.setNextPoseByIndex(SID_RELBOW, sElbow);
bioloid.setNextPoseByIndex(SID_LELBOW, 1024-sElbow);
sDelta = abs(bioloid.getNextPoseByIndex(SID_WRIST) - sWrist);
if (sDelta > sMaxDelta)
sMaxDelta = sDelta;
bioloid.setNextPoseByIndex(SID_WRIST, sWrist);
#ifdef OPT_WRISTROT
bioloid.setNextPoseByIndex(SID_WRISTROT, sWristRot);
#endif
bioloid.setNextPoseByIndex(SID_GRIP, sGrip);
// Save away the current positions...
g_sBase = sBase;
g_sShoulder = sShoulder;
g_sElbow = sElbow;
g_sWrist = sWrist;
g_sWristRot = sWristRot;
g_sGrip = sGrip;
// Now start the move - But first make sure we don't move too fast.
if (((long)sMaxDelta*wTime/1000L) > MAX_SERVO_DELTA_PERSEC) {
wTime = ((long)sMaxDelta*1000L)/ MAX_SERVO_DELTA_PERSEC;
}
bioloid.interpolateSetup(wTime);
// Do at least the first movement
bioloid.interpolateStep();
// And if asked to, wait for the previous move to complete...
if (fWait) {
while (bioloid.interpolating() > 0) {
bioloid.interpolateStep();
delay(3);
}
}
}
//===================================================================================================
// loop: Our main Loop!
//===================================================================================================
void loop() {
boolean fChanged = false;
if (command.ReadMsgs()) {
// See if the Arm is active yet...
if (g_fArmActive) {
sBase = g_sBase;
sShoulder = g_sShoulder;
sElbow = g_sElbow;
sWrist = g_sWrist;
sGrip = g_sGrip;
sWristRot = g_sWristRot;
if ((command.buttons & BUT_R1) && !(buttonsPrev & BUT_R1)) {
if (++g_bIKMode > IKM_BACKHOE)
g_bIKMode = 0;
// For now lets always move arm to the home position of the new input method...
// Later maybe we will get the current position and covert to the coordinate system
// of the current input method.
MoveArmToHome();
}
else if ((command.buttons & BUT_R2) && !(buttonsPrev & BUT_R2)) {
MoveArmToHome();
}
#ifdef DEBUG
if ((command.buttons & BUT_R3) && !(buttonsPrev & BUT_R3)) {
g_fDebugOutput = !g_fDebugOutput;
MSound( 1, 45, g_fDebugOutput? 3000 : 2000);
}
#endif
// Going to use L6 in combination with the right joystick to control both the gripper and the
// wrist rotate...
else if (command.buttons & BUT_L6) {
sGrip = min(max(sGrip + command.rightV/2, GRIP_MIN), GRIP_MAX);
sWristRot = min(max(g_sWristRot + command.rightH/6, WROT_MIN), WROT_MAX);
fChanged = (sGrip != g_sGrip) || (sWristRot != g_sWristRot);
}
else {
switch (g_bIKMode) {
case IKM_IK3D_CARTESIAN:
fChanged |= ProcessUserInput3D();
break;
case IKM_CYLINDRICAL:
fChanged |= ProcessUserInputCylindrical();
break;
case IKM_BACKHOE:
fChanged |= ProcessUserInputBackHoe();
break;
}
}
// If something changed and we are not in an error condition
if (fChanged && (g_bIKStatus != IKS_ERROR)) {
MoveArmTo(sBase, sShoulder, sElbow, sWrist, sWristRot, sGrip, 100, true);
}
else if (bioloid.interpolating() > 0) {
bioloid.interpolateStep();
}
}
else {
g_fArmActive = true;
MoveArmToHome();
}
buttonsPrev = command.buttons;
ulLastMsgTime = millis(); // remember when we last got a message...
}
else {
if (bioloid.interpolating() > 0) {
bioloid.interpolateStep();
}
// error see if we exceeded a timeout
if (g_fArmActive && ((millis() - ulLastMsgTime) > ARBOTIX_TO)) {
PutArmToSleep();
}
}
}
//==============================================================================
// BackgroundProcess:
// If using old Bioloid library may need to do some interpolation.
//==============================================================================
void ServoDriver::BackgroundProcess(void)
{
bioloid.interpolateStep(false);
}
word ServoDriver::GetBatteryVoltage(void) {
// The USB2AX is caching informatation for us, so simply ask it for the data. If this is still
// too much overhead, then we may want to only do this on timer...
// Lets cycle through the Tibia servos asking for voltages as they may be the ones doing the most work...
unsigned long uldt = millis() - g_ulTimeLastBatteryVoltage;
if ((uldt > VOLTAGE_MAX_TIME_BETWEEN_CALLS) || ((uldt > VOLTAGE_MIN_TIME_BETWEEN_CALLS && !bioloid.interpolating()))) {
word wVoltage = (word)ax12GetRegister(cPinTable[FIRSTFEMURPIN /*+ g_bLegVoltage++*/], AX_PRESENT_VOLTAGE, 1);
if (g_bLegVoltage == CNT_LEGS)
g_bLegVoltage = 0;
if (wVoltage && (wVoltage != 0xffff)) {
g_ulTimeLastBatteryVoltage = millis();
if (wVoltage != g_wLastVoltage) {
printf("Voltage: %d\n\r", wVoltage);
g_wLastVoltage = wVoltage;
}
} else if ((uldt > VOLTAGE_TIME_TO_ERROR) && (g_wLastVoltage != 0xffff)) {
printf("Voltage: error timeout");
g_wLastVoltage = 0xffff;
}
}
return ((g_wLastVoltage != (word)-1)? g_wLastVoltage*10 : (word)-1);
}
//--------------------------------------------------------------------
//Init
//--------------------------------------------------------------------
void ServoDriver::Init(void) {
// First lets get the actual servo positions for all of our servos...
g_fServosFree = true;
// We will duplicate and expand on the functionality of the bioloid readPose,
// function. In our loop we will set the IDs to that of our table, so they
// will be in the same order. Plus we will verify we have all of the servos
// If we care configured such that none of the servos in our loop have id #1 and
// we find that servo #1 and only one other is not found, we will assume that
// servo did the renumber to 1 problem and we will renumber it to the missing one.
// But again only if 1 servo is missing.
// Note: We are not saving the read positions, but the make sure servos are on will...
bioloid.poseSize(NUMSERVOS); // Method in this version
uint16_t w;
int count_missing = 0;
int missing_servo = -1;
bool servo_1_in_table = false;
for (int i = 0; i < NUMSERVOS; i++) {
// Set the id
bioloid.setId(i, cPinTable[i]);
if (cPinTable[i] == 1)
servo_1_in_table = true;
// Now try to get it's position
w = ax12GetRegister(cPinTable[i], AX_PRESENT_POSITION_L, 2);
if (w == 0xffff) {
// Try a second time to make sure.
delay(25);
w = ax12GetRegister(cPinTable[i], AX_PRESENT_POSITION_L, 2);
if (w == 0xffff) {
// We have a failure
printf("Servo(%d): %d not found", i, cPinTable[i]);
if (++count_missing == 1)
missing_servo = cPinTable[i];
}
}
delay(25);
}
// Now see if we should try to recover from a potential servo that renumbered itself back to 1.
if (count_missing)
printf("ERROR: Servo driver init: %d servos missing\n", count_missing);
if ((count_missing == 1) && !servo_1_in_table) {
if (dxl_read_word(1, AX_PRESENT_POSITION_L) != 0xffff) {
printf("Servo recovery: Servo 1 found - setting id to %d\n", missing_servo);
dxl_write_byte(1, AX_ID, missing_servo);
}
}
#ifdef USB2AX_REG_VOLTAGE
ax12SetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, cPinTable[FIRSTFEMURPIN]);
#endif
g_fAXSpeedControl = false;
#ifdef OPT_GPPLAYER
_fGPEnabled = true; // assume we support it.
#endif
}
