void StartRun(void) { int i; // calibrate gyroscope (if not done) if (!bGyroCalibratedFlag) CalibrateGyro(); // reset alignment values linePosition = 0; prevLinePosition = 0; // reset side sensors variables for (i = 0; i < NUM_SIDE_SENSORS; i++) { markerCnt[i] = 0; bSideMarkerFlag[i] = FALSE; bSideSenDisableFlag[i] = FALSE; posSideSen[i] = 0; } // reset speed profile ResetSpeedProfileData(); // reset flags bJunctionFlag = FALSE; posJunction = 0; // enable motor and assert run flag EnableMotor(); bRunFlag = TRUE; }
void DoSpeedProfile() { if (bWheelMotorEnable) { DoMoveCommand(); // speed profile UpdateCurSpeed(); // update curSpeed[] according to targetSpeed[] UpdateWheelPos(); UpdateRobotPos(); // To update the robot's current position based on the robot } else ResetSpeedProfileData(); }
void DoSpeedProfile() { if (bMotorEnableFlag) { DoMoveCommand(); // execute speed profile UpdateCurSpeed(); // update current speed according to target speed UpdateWheelPos(); // To update robot's current position (for use with speed profile) // UpdateRobotPos(); // To update the robot's current position // if (curSpeed[1] || curSpeed[0]) { // LogDataFn(debugData0, debugData1, curSpeed[0], curSpeed[1]); // LogDataFn(curSpeed[0], curSpeed[1], curTargetPos[0], finalPos[0]); // } } else { ResetSpeedProfileData(); } }
void StopRobot(void) { ResetSpeedProfileData(); }