TerminalPlugin::TerminalPlugin() : QObject(), JuffPlugin() { w_ = new QTermWidget(); w_->setScrollBarPosition(QTermWidget::ScrollBarRight); w_->setWindowTitle(tr("Terminal")); init(); termAct_ = new QAction(tr("Show/Hide terminal"), 0); termAct_->setShortcut(QKeySequence("Ctrl+Alt+T")); connect(termAct_, SIGNAL(triggered()), SLOT(showTerminal())); }
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; }