/*********************************************************************
* @fn Util_isActive
*
* @brief Determine if a clock is currently active.
*
* @param pClock - pointer to clock struct
*
* @return TRUE if Clock is currently active
FALSE otherwise
*/
bool Util_isActive(Clock_Struct *pClock)
{
Clock_Handle handle = Clock_handle(pClock);
// Start clock instance
return Clock_isActive(handle);
}
/*********************************************************************
* @fn Util_rescheduleClock
*
* @brief Reschedule a clock by changing the timeout and period values.
*
* @param pClock - pointer to clock struct
* @param clockPeriod - longevity of clock timer in milliseconds
* @return none
*/
void Util_rescheduleClock(Clock_Struct *pClock, uint32_t clockPeriod)
{
bool running;
uint32_t clockTicks;
Clock_Handle handle;
handle = Clock_handle(pClock);
running = Clock_isActive(handle);
if (running)
{
Clock_stop(handle);
}
// Convert period in milliseconds to ticks.
clockTicks = clockPeriod * (1000 / Clock_tickPeriod);
Clock_setTimeout(handle, clockTicks);
Clock_setPeriod(handle, clockTicks);
if (running)
{
Clock_start(handle);
}
}
/*********************************************************************
* @fn Util_restartClock
*
* @brief Restart a clock by changing the timeout.
*
* @param pClock - pointer to clock struct
* @param clockTimeout - longevity of clock timer in milliseconds
*
* @return none
*/
void Util_restartClock(Clock_Struct *pClock, uint32_t clockTimeout)
{
uint32_t clockTicks;
Clock_Handle handle;
handle = Clock_handle(pClock);
if (Clock_isActive(handle))
{
// Stop clock first
Clock_stop(handle);
}
// Convert timeout in milliseconds to ticks.
clockTicks = clockTimeout * (1000 / Clock_tickPeriod);
// Set the initial timeout
Clock_setTimeout(handle, clockTicks);
// Start clock instance
Clock_start(handle);
}
void DriveFn (){
static int lightFlag = 0;
while(1){
Semaphore_pend(DriveSema, BIOS_WAIT_FOREVER);
switch (drive_state) {
case START:
//enable motors forward.
PWMOutputState(PWM0_BASE, MOTOR_LF_EN , true);
PWMOutputState(PWM0_BASE, MOTOR_RF_EN , true);
//accelerate to speed
setSpeed (200, MOTOR_RF_EN);
setSpeed (200, MOTOR_LF_EN);
drive_state = FOLLOW1;
GPIO_write(Board_LED0, Board_LED_ON);
break;
//Follow1,2 and 3 grouped together since they do the same thing.
//They do progress depending on the lines the robot has passed, but that
//is handled in another area.
case FOLLOW1:
case FOLLOW2:
case FOLLOW3:
prev_follow_state = drive_state;
if (lightFlag == 0)
//acquire data
lightFlag = !lightFlag;
if (!wall_det) {
drive_state = INTERSECTION;
}
else if (front_wall_det == 1){
drive_state = UTURN; //stop if front wall detected
}
//read right wall distance
PID_Algorithm();
break;
case INTERSECTION:
if (Clock_isActive(DataClock) ){ //if collecting data
Clock_stop(DataClock); //don't collect data in intersection
motor_RightTurn();
Clock_start(DataClock);
}
else //if not collecting data
motor_RightTurn(); //move along
break;
case UTURN:
if (Clock_isActive(DataClock) ){ //if collecting data
Clock_stop(DataClock); //don't collect data in uturn
motor_Uturn();
Clock_start(DataClock);
}
else //if not collecting data
motor_Uturn(); //move along
break;
case STOP:
//decelerate to a stop
setSpeed (0, MOTOR_LF_EN);
setSpeed (0, MOTOR_RF_EN);
StopDrivingFn();
drive_state = OFF;
break;
case OFF:
//disable motors
PWMOutputState(PWM0_BASE, MOTOR_LF_EN , false);
PWMOutputState(PWM0_BASE, MOTOR_RF_EN , false);
break;
}
}
}