void DeltaServos::upDown( int cycleTime ) {
//int STEPS = 32;
int STEPS = 255;
int INCR = 255/STEPS;
int dly = (cycleTime/STEPS/2);
if (dly < servoDelay) dly = servoDelay;
int i = 0;
while ( i < 255 ) {
brain.setLED( i,i,i );
updateServos( i,i,i );
delay(dly-servoDelay);
i+= INCR;
}
delay(500);
i = 255;
while ( i > 0 ) {
brain.setLED( i,i,i );
updateServos( i,i,i );
delay(dly-servoDelay);
i-= INCR;
}
delay(500);
}
void ModeLibrary::robotRamp( int steps, double sa, double sb, double sc ) {
double ra = (servoA - sa)/steps;
double rb = (servoB - sb)/steps;
double rc = (servoC - sc)/steps;
if ( steps < 5 ) {
for ( int i=0; i< steps; i++ ) {
servoA-=ra;
servoB-=rb;
servoC-=rc;
updateServos();
delay(30); // This can be a little too fast.
}
} else {
for ( int i=0; i< steps/5; i++ ) {
servoA-=ra;
servoB-=rb;
servoC-=rc;
updateServos();
}
for ( int i=0; i< steps/5; i++ ) {
servoA-=ra*3.0;
servoB-=rb*3.0;
servoC-=rc*3.0;
updateServos();
}
for ( int i=0; i< steps/5; i++ ) {
servoA-=ra;
servoB-=rb;
servoC-=rc;
updateServos();
}
}
}
ServoGauges::ServoGauges(QObject *parent) :
QObject(parent)
{
speed = depth = 0;
if(controller.openSerial()) {
connect(&updateTimer, SIGNAL(timeout()), this, SLOT(updateServos()));
updateTimer.setSingleShot(false);
updateTimer.setInterval(100);
updateTimer.start();
}
}
void ModeLibrary::swirl( int rate ) {
double frame = 0.0;
robotRamp( 40, sin(frame/100.0), sin((frame+100.0)/100.0), sin((frame+200.0)/100.0) );
while (frame < 628) {
servoA = sin(frame/100.0);
servoB = sin((frame+100.0)/100.0);
servoC = sin((frame+200.0)/100.0);
frame+=(double)rate;
updateServos();
}
}
/**
* Dip straight down then return back.
* nBobs :: Number of times to bob
* pmn1 :: first pause minimum in milliseconds (mS)
* pmx1 :: first pause maximum in milliseconds (mS)
* pmn2 :: first pause minimum in milliseconds (mS)
* pmx2 :: first pause maximum in milliseconds (mS)
*/
void ModeLibrary::bob( int nBobs, int pmn1, int pmx1, int pmn2, int pmx2 ) {
double svA = servoA;
double svB = servoB;
double svC = servoC;
for ( int i=0; i< nBobs; i++) {
servoA -= 0.2;
servoB -= 0.2;
servoC -= 0.2;
updateServos();
//robotDo(servoA, servoB, servoC, ledR, ledG, ledB );
}
pause(pmn1, pmx1);
servoA = svA;
servoB = svB;
servoC = svC;
updateServos();
//robotDo(servoA, servoB, servoC, ledR, ledG, ledB );
pause(pmn2, pmx2);
}
//update the servos & non-drivetrain motors
void udServos(){
//control the hooks on the sides of the robot
/* if(joy2Btn(1))
motor[hook]=HOOK_POWER;
else if(joy2Btn(2))
motor[hook]=-HOOK_POWER;
else
motor[hook]=0;*/
if(joy2Btn(1))
setRightHook(true);
else if(joy2Btn(2))
setRightHook(false);
if(joy2Btn(4))
motor[sweeper]=SWEEPER_POWER;
else if(joy2Btn(3))
motor[sweeper]=-SWEEPER_POWER;
else
motor[sweeper]=0;
//move lift
if(joy2Btn(5))
setLiftPos(-1,false);//down
else if(joy2Btn(6))
setLiftPos(1,true);//up
else
setLiftPos(0,true);//stop if no button is pressed
//control conveyor
if(joy2Btn(7))
moveConveyor(CONVEYOR_UP);//intake
else if(joy2Btn(8))
moveConveyor(CONVEYOR_DOWN);//outtake
else
moveConveyor(CONVEYOR_STOP);
if(abs(joystick.joy2_x1)>joyTol)
setDump(joystick.joy2_x1/-6);
else
//tilt the PVC carrier thing
setDump(10);
//move the conveyor also with the joystick
if(abs(joystick.joy2_y2)>joyTol)
moveConveyor(joystick.joy2_y2);
//do magical things with the MC library
updateServos();
}
void init(){
Stop();//stop all of the robot's motors
moveConveyor(false);
setDump(0);//flatten the PVC thing
setRightHook(false);
activateServos();
updateServos();//magic
//reset motor encoders
nMotorEncoder[liftMotor]=0;
nMotorEncoder[wheelA]=0;
nMotorEncoder[wheelB]=0;
nMotorEncoder[wheelC]=0;
nMotorEncoder[wheelD]=0;
return;
}
void ArmServosSpeedControlled::setPosition(int joint1Angle, int joint2Angle, int joint3Angle, int joint4Angle, int joint5Angle) {
int absoluteTimeMs = millis();
_moveStartTimes[1] = absoluteTimeMs;
_moveStartTimes[2] = absoluteTimeMs;
_moveStartTimes[3] = absoluteTimeMs;
_moveStartTimes[4] = absoluteTimeMs;
_moveStartTimes[5] = absoluteTimeMs;
_moveStartJointAngles[1] = _servoAngles[1];
_moveStartJointAngles[2] = _servoAngles[2];
_moveStartJointAngles[3] = _servoAngles[3];
_moveStartJointAngles[4] = _servoAngles[4];
_moveStartJointAngles[5] = _servoAngles[5];
_targetJointAngles[1] = joint1Angle;
_targetJointAngles[2] = joint2Angle;
_targetJointAngles[3] = joint3Angle;
_targetJointAngles[4] = joint4Angle;
_targetJointAngles[5] = joint5Angle;
updateServos();
}
/**
* Jab in some direction and return back
*/
void ModeLibrary::jab() {
// Remember our starting position.
double svA = servoA;
double svB = servoB;
double svC = servoC;
// Pick a random direction to jab in.
switch( rand()%6 ) {
case 0:
servoC =- 0.5;
updateServos();
robotRamp(2, 1.0, 1.0, -1.0 );
break;
case 1:
servoB =- 0.5;
updateServos();
robotRamp(2, 1.0, -1.0, 1.0 );
break;
case 2:
servoB =- 0.5;
servoC =- 0.5;
updateServos();
robotRamp(2, 1.0, -1.0, -1.0 );
break;
case 3:
servoA =- 0.5;
servoC =- 0.5;
updateServos();
robotRamp(2, -1.0, 1.0, -1.0 );
break;
case 4:
servoA =- 0.5;
servoB =- 0.5;
updateServos();
robotRamp(2, -1.0, -1.0, 1.0 );
break;
case 5:
servoA =- 0.5;
updateServos();
robotRamp(2, -1.0, 1.0, 1.0 );
break;
}
pause(100, 300);
// Sometimes recoil from jab
if ( coinFlip() ) {
robotRamp( 6, svA, svB, svC );
pause(1, 300);
}
}
void packetReceived() {
if (rx.cmd == 'A') {
if (!x2Connection) {
// SYN packet received
if (rx.data[0] & A_FLAGS_SYN) {
x2Connection = X2_CONN_SYN;
// Send back SYN+ACK packet with team number echoed
aPacket[0] = A_FLAGS_SYN | A_FLAGS_ACK;
aPacket[3] = rx.data[3];
aPacket[4] = rx.data[4];
sendPacket('A', A_PACKET_LENGTH, aPacket);
}
} else if (x2Connection == X2_CONN_SYN) {
if (rx.data[0] & A_FLAGS_ACK) {
x2Connection = X2_CONN_ESTABLISHED;
// store User Processor software version ID
gPacket[4] = rx.data[1];
gPacket[5] = rx.data[2];
// store team #
teamNo = (rx.data[3] << 8) | rx.data[4];
x2OkToSend = TRUE;
}
}
} else if (rx.cmd == 'Y') {
// Don't update motor or servo values in disable mode
if (ROBOTMODE & ROBOT_DISABLE) return;
//printf ("packet received\n");
x2OkToSend = TRUE;
// Update motor values
motors.trex[0][0] = rx.data[0];
motors.trex[0][1] = rx.data[1];
// TODO rx.data[2] is TReX unidirectional motor
// rx.data[3:5] is reserved for future expansion
motors.trex[1][0] = rx.data[3];
motors.trex[1][1] = rx.data[4];
updateTREX();
// rx.data[6:7] are X2 motor values
// rx.data[8:13] are sent to servo controller
servos.positions[0] = rx.data[8];
servos.positions[1] = rx.data[9];
servos.positions[2] = rx.data[10];
servos.positions[3] = rx.data[11];
servos.positions[4] = rx.data[12];
servos.positions[5] = rx.data[13];
updateServos();
//printf("motor1: %d, motor2: %d, servo1: %d, servo2: %d\n", motors.trex[0], motors.trex[1], servos.positions[0], servos.positions[1]);
// rx.data[14:15] are X2 servo values
// rx.data[16:17] are battery voltage
// store battery voltage in field control response packet
gPacket[6] = rx.data[16];
gPacket[7] = rx.data[17];
// send J response packet to UDP Interface
memcpy(jPacket, rx.data, 18);
sendInterfaceSocket(jPacket, J_PACKET_LENGTH);
// send G response packet to field control for telemetry (battery voltage)
sendFieldSocket(gPacket, G_PACKET_LENGTH);
}
}
u8 PhoenixCore::loop(void)
{
bool allDown;
long lBodyX; //Output Position X of feet with Rotation
long lBodyY; //Output Position Y of feet with Rotation
long lBodyZ; //Output Position Z of feet with Rotation
u8 ret = STATUS_OK;
//Start time
mTimerStart = millis();
if (mCommitTime != 0) {
if (mTimerStart >= mCommitTime) {
mServo->commit(mCurServoMoveTime);
mCommitTime = 0;
mTimerStart = millis();
} else {
return ret;
}
}
// every 500ms
if (mTimerStart - mTimerLastCheck > 500) {
mCurVolt = mServo->getBattVolt();
mTimerLastCheck = mTimerStart;
if (mCurVolt < CONFIG_VOLT_OFF) {
mVoltWarnBeepCnt++;
if (mVoltWarnBeepCnt > 10) {
return STATUS_BATT_FAIL;
}
else
ret |= STATUS_BATT_WARN;
} else {
mVoltWarnBeepCnt = 0;
}
}
if (mBoolUpsideDown) {
mPtrCtrlState->c3dTravelLen.x = -mPtrCtrlState->c3dTravelLen.x;
mPtrCtrlState->c3dBodyPos.x = -mPtrCtrlState->c3dBodyPos.x;
mPtrCtrlState->c3dSingleLeg.x = -mPtrCtrlState->c3dSingleLeg.x;
mPtrCtrlState->c3dBodyRot.z = -mPtrCtrlState->c3dBodyRot.z;
}
//Single leg control
allDown = ctrlSingleLeg();
//doGait
doGaitSeq();
//Balance calculations
mTotalTransX = 0; //reset values used for calculation of balance
mTotalTransZ = 0;
mTotalTransY = 0;
mTotalXBal1 = 0;
mTotalYBal1 = 0;
mTotalZBal1 = 0;
if (mPtrCtrlState->fBalanceMode) {
for (u8 i = 0; i < CONFIG_NUM_LEGS / 2; i++) { // balance calculations for all Right legs
calcBalOneLeg(i, -mLegPosXs[i]+mGaitPosXs[i],
mLegPosZs[i]+mGaitPosZs[i],
(mLegPosYs[i]-(s16)pgm_read_word(&TBL_INT_POS_Y[i]))+mGaitPosYs[i]);
}
for (u8 i = CONFIG_NUM_LEGS / 2; i < CONFIG_NUM_LEGS; i++) { // balance calculations for all Right legs
calcBalOneLeg(i, mLegPosXs[i]+mGaitPosXs[i],
mLegPosZs[i]+mGaitPosZs[i],
(mLegPosYs[i]-(s16)pgm_read_word(&TBL_INT_POS_Y[i]))+mGaitPosYs[i]);
}
balanceBody();
}
//Do IK for all Right legs
for (u8 i = 0; i < CONFIG_NUM_LEGS / 2; i++) {
getBodyIK(i,
-mLegPosXs[i]+mPtrCtrlState->c3dBodyPos.x+mGaitPosXs[i] - mTotalTransX,
mLegPosZs[i]+mPtrCtrlState->c3dBodyPos.z+mGaitPosZs[i] - mTotalTransZ,
mLegPosYs[i]+mPtrCtrlState->c3dBodyPos.y+mGaitPosYs[i] - mTotalTransY,
mGaitRotYs[i],
&lBodyX, &lBodyY, &lBodyZ);
ret |= getLegIK(i, mLegPosXs[i]-mPtrCtrlState->c3dBodyPos.x+lBodyX-(mGaitPosXs[i] - mTotalTransX),
mLegPosYs[i]+mPtrCtrlState->c3dBodyPos.y-lBodyY+mGaitPosYs[i] - mTotalTransY,
mLegPosZs[i]+mPtrCtrlState->c3dBodyPos.z-lBodyZ+mGaitPosZs[i] - mTotalTransZ);
}
//Do IK for all Left legs
for (u8 i = CONFIG_NUM_LEGS / 2; i < CONFIG_NUM_LEGS; i++) {
getBodyIK(i,
mLegPosXs[i]-mPtrCtrlState->c3dBodyPos.x+mGaitPosXs[i] - mTotalTransX,
mLegPosZs[i]+mPtrCtrlState->c3dBodyPos.z+mGaitPosZs[i] - mTotalTransZ,
mLegPosYs[i]+mPtrCtrlState->c3dBodyPos.y+mGaitPosYs[i] - mTotalTransY,
mGaitRotYs[i],
&lBodyX, &lBodyY, &lBodyZ);
ret |= getLegIK(i, mLegPosXs[i]+mPtrCtrlState->c3dBodyPos.x-lBodyX+mGaitPosXs[i] - mTotalTransX,
mLegPosYs[i]+mPtrCtrlState->c3dBodyPos.y-lBodyY+mGaitPosYs[i] - mTotalTransY,
mLegPosZs[i]+mPtrCtrlState->c3dBodyPos.z-lBodyZ+mGaitPosZs[i] - mTotalTransZ);
}
if (mBoolUpsideDown) { //Need to set them back for not messing with the smoothControl
mPtrCtrlState->c3dBodyPos.x = -mPtrCtrlState->c3dBodyPos.x;
mPtrCtrlState->c3dSingleLeg.x = -mPtrCtrlState->c3dSingleLeg.x;
mPtrCtrlState->c3dBodyRot.z = -mPtrCtrlState->c3dBodyRot.z;
}
//Check mechanical limits
validateAngles();
//Drive Servos
if (mPtrCtrlState->fHexOn) {
//Calculate Servo Move time
if ((abs(mPtrCtrlState->c3dTravelLen.x) > CONFIG_TRAVEL_DEAD_ZONE) ||
(abs(mPtrCtrlState->c3dTravelLen.z) > CONFIG_TRAVEL_DEAD_ZONE) ||
(abs(mPtrCtrlState->c3dTravelLen.y * 2) > CONFIG_TRAVEL_DEAD_ZONE)) {
mCurServoMoveTime = mNormGaitSpeed + (mPtrCtrlState->bInputTimeDelay * 2) + mPtrCtrlState->wSpeedControl;
//Add aditional delay when Balance mode is on
if (mPtrCtrlState->fBalanceMode)
mCurServoMoveTime = mCurServoMoveTime + 100;
} else { //Movement speed excl. Walking
mCurServoMoveTime = 200 + mPtrCtrlState->wSpeedControl;
}
// note we broke up the servo driver into start/commit that way we can output all of the servo information
// before we wait and only have the termination information to output after the wait. That way we hopefully
// be more accurate with our timings...
updateServos();
// See if we need to sync our processor with the servo driver while walking to ensure the prev is completed
// before sending the next one
// Finding any incident of GaitPos/Rot <>0:
for (u8 i = 0; i < CONFIG_NUM_LEGS; i++) {
if ( (mGaitPosXs[i] > GP_DIFF_LIMIT) || (mGaitPosXs[i] < -GP_DIFF_LIMIT) ||
(mGaitPosZs[i] > GP_DIFF_LIMIT) || (mGaitPosZs[i] < -GP_DIFF_LIMIT) ||
(mGaitRotYs[i] > GP_DIFF_LIMIT) || (mGaitRotYs[i] < -GP_DIFF_LIMIT)) {
mExtraCycle = mNrLiftedPos + 1;//For making sure that we are using timed move until all legs are down
break;
}
}
//printf(F("ExtraCycle:%d\n"), mExtraCycle);
if (mExtraCycle > 0) {
mExtraCycle--;
mBoolWalking = (mExtraCycle != 0);
mCommitTime = mTimerStart + mOldServoMoveTime + CONFIG_SERVO_MARGIN;
//printf(F("Next1:%ld\n"), mCommitTime);
if (mBoolDbgOutput) {
printf(F("BRX:%d, Walk:%d, GS:%d\n"), mPtrCtrlState->c3dBodyRot.x, mBoolWalking, mGaitStep);
printf(F("LEFT GPX:%5d, GPY:%5d, GPZ:%5d\n"), mGaitPosXs[IDX_LF], mGaitPosYs[IDX_LF], mGaitPosZs[IDX_LF]);
printf(F("RIGHT GPX:%5d, GPY:%5d, GPZ:%5d\n"), mGaitPosXs[IDX_RF], mGaitPosYs[IDX_RF], mGaitPosZs[IDX_RF]);
}
} else {
// commit immediately
mCommitTime = mTimerStart + mOldServoMoveTime + CONFIG_SERVO_MARGIN;
//printf(F("Next2:%ld\n"), mCommitTime);
}
if (mBoolDbgOutput) {
printf(F("TY:%5d, LFZ:%5d\n"), mTotalYBal1, mLegPosZs[IDX_LF]);
}
} else {
//Turn the bot off - May need to add ajust here...
if (mPtrCtrlState->fHexOnOld) {
printf(F("RESET LEGS !!!\n"));
mCurServoMoveTime = 600;
updateServos();
mServo->commit(mCurServoMoveTime);
delay(600);
} else {
mServo->release();
}
// We also have a simple debug monitor that allows us to
// check things. call it here..
#ifdef CONFIG_TERMINAL
if (showTerminal())
return ret;
#endif
}
mOldServoMoveTime = mCurServoMoveTime;
return ret;
}
void DeltaServos::updateServos( float * abc ) {
updateServos(abc[0], abc[1], abc[2]);
}
void DeltaServos::updateServos( float sa, float sb, float sc ) {
updateServos( (int)sa, (int)sb, (int)sc );
}
