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();
}
