int main(void)
{
Timer_Init(); //Initialize timers
Ultrasonic_Init(); //Initialize sensors
LED_Init(); //Initialize LEDs
UART_Init(); //Initialize UART
Motors_Init(); //Initialize motors
sei(); //Enable interrupts
while(1) //Main loop
{
state = 1;
if (check_command(REMOTECONTROLMODE))
{
state = 0;
manual();
}
else if (check_command(LATERALPARKINGMODE))
{
state = 0;
lateral_parking();
}
else if (check_command(MAINPARKINGMODE))
{
state = 0;
random_parking();
}
else if (check_command(FIXEDPARKINGMODE))
{
state = 0;
fixed_parking();
}
else if (check_command(OBSTACLEDODGEMODE))
{
state = 0;
obstacle_dodge();
}
else if (check_command(FOLLOWMODE))
{
state = 0;
follow();
}/*
else if (confirm_input(DANCE))
{
state = 4;
dance();
}*/
}
}
void Setup() {
/* Initialize LCD */
lcd_initialize();
/* Clear LCD */
lcd_clear();
/* Display message on LCD */
lcd_buffer_print(LCD_LINE2, " TEST ");
/* Initialize motors */
Motors_Init();
/* Turn on motors relay */
Motors_On();
/* Send arm signal to motors */
Motor_Arm(MOTOR_UPPER);
Motor_Arm(MOTOR_BOTTOM);
/* Initialize servos */
Servos_Init();
/* Initialize sonar */
sonarInitialize(); //must be initialized before IIC, otherwise it will not work
/* Setup the 12-bit A/D converter */
S12ADC_init();
/* Initialize I2C with control */
riic_ret_t iic_ret = RIIC_OK;
iic_ret |= riic_master_init();
while (RIIC_OK != iic_ret) {
nop(); /* Failure to initialize here means demo can not proceed. */
}
/* Setup Compare Match Timer */
CMT_init();
/* Initialize PID structure used for PID properties */
PID_Init(&z_axis_PID, 0.7, 0.05, 0.30, dt, 0, 0.5); //0.7 0.05 0.15
PID_Init(&Pitch_PID, 1, 0, 0.01, dt, -30, 30);
PID_Init(&Roll_PID, 1, 0, 0.01, dt, -30, 30);
Init_AVG(0, &pitchAVG);
Init_AVG(0, &rollAVG);
/* Make the port connected to SW1 an input */
PORT4.PDR.BIT.B0 = 0;
/*MPU6050 Initialization*/
MPU6050_Test_I2C();
Setup_MPU6050();
Calibrate_Gyros();
// Calibrate_Accel();
/*Kalman Initialization*/
init_Kalman();
//MS5611-01BA01 init
// MS5611_Init();
desiredState.key.motor_diff_us = 0;
desiredState.key.abs.pos.z = 0.20;
altitudeValue = 200;
mainWDT = WDT_Init(500, Fallback);
WDT_Start(&mainWDT);
sonarWDT = WDT_Init(60, Sonar_Fallback);
WDT_Start(&sonarWDT);
}
