int main (void)
{
InitUart(); //Init uart
initInput(); //Init interrupt to start button, stop button and speed sensor
initMotorPWM(); //Init Fast PWM on Timer 0 (Pin PE1)
initSpeedTimer(); //Init Timer 1 (16bit normal mode)
initCurrentADC(); //Init ADC on PC5
//Init speed array
for (int i = 0; i<(SPEED_ARRAY_SIZE); i++)
{
speedArray[i]=65535;
}
while (1)
{
//Switch to stop state if something is rotten
Protection(&Sm_State, &speed);
switch (Sm_State)
{
case STATE_STOP:
Stop(&motor);
break;
case STATE_RAMP_INIT:
RampInit(&Sm_State, &motor);
break;
case STATE_RAMP_SEARCH:
RampSearch(&Sm_State, &motor, &speed, &lastSpeed);
break;
case STATE_ACC:
Acc(&Sm_State,&motor, speedArray);
break;
case STATE_COAST:
Coast(&Sm_State, &motor, &speed);
break;
default:
Sm_State = STATE_STOP;
break;
}
}
}
void Motor::Init( int d1Pin, int d2Pin, int enablePin )
{
if (( m_d1 = tpu_request_channel( 0, d1Pin )) == NULL )
{
LOG_ERROR(( "Unable to get TPU pin %d for %s Motor D1", d1Pin, m_name ));
return;
}
if (( m_d2 = tpu_request_channel( 0, d2Pin )) == NULL )
{
LOG_ERROR(( "Unable to get TPU pin %d for %s Motor D2", d2Pin, m_name ));
return;
}
if (( m_enable = tpu_request_channel( 0, enablePin )) == NULL )
{
LOG_ERROR(( "Unable to get TPU pin %d for %s Motor Enable", enablePin, m_name ));
return;
}
SetSpeed( 0 );
SetFrequency( MOTOR_DEFAULT_PWM_FREQ );
Coast();
} // Init
