/*******************************************************************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE: Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode. It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM: main.c file, main() routine when in Autonomous mode
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void User_Autonomous_Code(void)
{
/* Initialize all PWMs and Relays when entering Autonomous mode, or else it
will be stuck with the last values mapped from the joysticks. Remember,
even when Disabled it is reading inputs from the Operator Interface.
*/
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
init_wings();
relay1_fwd = relay1_rev = relay2_fwd = relay2_rev = 0;
relay3_fwd = relay3_rev = relay4_fwd = relay4_rev = 0;
relay5_fwd = relay5_rev = relay6_fwd = relay6_rev = 0;
relay7_fwd = relay7_rev = relay8_fwd = relay8_rev = 0;
while (autonomous_mode) /* DO NOT CHANGE! */
{
if (statusflag.NEW_SPI_DATA) /* 26.2ms loop area */
{
Getdata(&rxdata); /* DO NOT DELETE, or you will be stuck here forever! */
/* Add your own autonomous code here. */
Process_Autonomous_Code();
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
Putdata(&txdata); /* DO NOT DELETE, or you will get no PWM outputs! */
}
}
}
/**
* Checks for init
* return 0 if the command was "init"
*/
int check_for_init(void){
if(veh.server_reply[0] == 'i'){
if (veh.state == VEH_UNINIT) {
// init vehicle
printf("**************************************************\n");
printf("I AM INITIALIZING THE VEHICLE\n");
printf("I WILL INITIALIZE IN 2 STAGES\n");
printf("THIS CAN TAKE UP TO 30 SECONDS\n");
#if !DEBUG
init_wings(); // MUST BE DONE AT LEAST ONCE AFTER A RESET DEVICE
#endif
printf("PREPARING TO A CONTROL SWITCH ROUTINE\n");
#if !DEBUG
sleep(3);
switch_control();
#endif
printf("PREPARING TO RETURN CONTROL SWITCH ROUTINE\n");
#if !DEBUG
sleep(3);
switch_control();
#endif
printf("SLOW START INITIALIZATION COMPLETED\n");
printf("**************************************************\n\n");
veh.state = VEH_INIT;
}
else {
printf("**************************************************\n");
printf("Already initialized.\n");
printf("**************************************************\n");
}
// prepare response
sprintf(veh.message, "%s", init);
// command found
return 0;
}
else {
// continue search
return 1;
}
}
/*******************************************************************************
* FUNCTION NAME: User_Initialization
* PURPOSE: This routine is called first (and only once) in the Main function.
* You may modify and add to this function.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void User_Initialization (void)
{
init_wings();
User_Proc_Is_Ready(); /* DO NOT CHANGE! - last line of User_Initialization */
}