int main()
{
pin_config_init();
timing_interrupt_config();
//ir_finder_analog_setup();
//analog_ultrasonic_setup();
//analog_ultrasonic_setup();
//ultrasonic_setup();
//config_pwm_14();
// ANSA = 1;
//ANSB = 1; //Digital buffer is active
//_TRISA0 = 1;
//_TRISB2 = 1;
//_TRISA2 = 1;
//_TRISA0 = 1;
//_TRISA1 = 1;
float angle = find_normal_angle();
STATE = 0;
while(1)
{
// switch(findCenterState)
// {
// case 0:
// if(find_normal()) //First normal found
// {
// findCenterState = 1;
// }
// else //Keep looking
// {
// //do nothing
// }
// break;
// case 1:
// if(find_24()) //First 24" found
// {
// findCenterState = 2;
// }
// else //Keep looking
// {
// //do nothing
// }
// break;
// case 2:
// if(turn_degrees(90)) //Turned 90 degrees
// {
// findCenterState = 3;
// }
// else
// {
// //do nothing
// }
// break;
// case 3:
// if(find_24()) //Second 24" found
// {
// findCenterState = 0;
// //STATE = findLoader;
//
// delay(10000);
// }
// else
// {
// //do nothing
// }
// break;
// }
switch(STATE)
{
case 0:
if(turn_degrees(angle))//if(find_normal_analog())
{
STATE = 1;
}
break;
case 1:
delay(5000);
STATE = 2;
break;
case 2:
if(find_inches(24.0))
{
delay(5000);
STATE = 3;
}
break;
case 3:
if(turn_degrees(90))
{
delay(5000);
STATE = 4;
}
break;
case 4:
if(find_inches(24.0))
{
delay(5000);
STATE = 5;
}
break;
case 5:
//do nothing...
break;
}
}
}
int main(void)
{
LED_INIT;
//servoTx;
sei();
USART_init();
MPU_init();
SERVO_init();
init_counters();
initvar();
SERVO_update_EEPROM(BROADCASTING_ID);
wait(10);
move_to_std();
wait_until_gyro_stable();
LED0_ON;
USART_send_message("Gyro Stable");
// ------ TESTCODE FOR READING SERVO -------
//servoGoto(1, 3.14/3, 0x200);
SERVO_update_EEPROM(BROADCASTING_ID); // NOTE: needs to run once for SERVO_get position to work
//----------------------------
reset_counter_1();
set_counter_1(3000);
uint8_t readyCounter = 0;
while(1)
{
MPU_update();
if(USART_get_turn_flag())
{
turn_degrees(USART_get_turn_angle(), USART_get_turn_dir());
}
if(USART_get_climb_flag())
{
climb();
}
uint8_t r = USART_getRotation();
uint8_t s = USART_getSpeed();
uint8_t d = USART_getDirection();
if(s != 0 || r != 50)
{
std_pos_flag = 0;
reset_counter_1();
readyCounter = 3;
}
move_robot(d, r, s);
if(r == 50 && s == 0 && d == 0 && readyCounter)
{
cli();
USART_send_ready();
sei();
readyCounter--;
}
if(move_to_std_flag == 1)
{
move_to_std_flag = 0;
move_to_std();
}
/*climb();
for(int i = 0; i < 10; ++i)
{
move_robot(0,50,100);
//wait(2000);
}
*/
/*
change_z(-120);
move_to_std();
turn_degrees(180,1);
// Takes a predecided number of steps forward
// This is good when testing different things.
wait(100);
for(int i = 0; i < 10; ++i)
{
move_robot(0,50,100);
//wait(2000);
}
*/
USART_decode_rx_fifo();
}
}
