int main()
{
pic_init();
logging_init();
button_init();
bumper_init();
pwm_init();
motor_timer_init();
button_timer_init();
sound_config_timer_init();
//put_str_ln("Initialising DMA...");
//init_DMA();
//put_str_ln("Initialising ADC...");
init_ADC();
// setup des interrupts
INTCONSET = _INTCON_MVEC_MASK;
__builtin_enable_interrupts();
if (VERBOSE_PIC_STATUS)
put_str_ln("Ready.");
while (1)
{
WDTCONbits.WDTCLR = 1; // ecrire un 1 dans ce bit force la reinitialisation du watchdog
}
}
int motor_rtctl_init(void)
{
int ret = motor_pwm_init();
if (ret) {
return ret;
}
motor_timer_init();
ret = motor_adc_init();
if (ret) {
return ret;
}
motor_forced_rotation_detector_init();
configure();
motor_rtctl_stop();
return 0;
}
void Analyse_And_Move()
{
// Mic 1 is the first sequentially sampled,
// Mic 3 is the last sequentially sampled,
// so a dely may exist between mic 1 and mic 3
// a sound may be seen on mic 1 and not mic 3
// even if they receive it at the exact same moment
if (VERBOSE_MIC_SOFTWARE)
{
log_key_val("m1", buffer[2][0] * 128);
log_key_val("m2", buffer[1][0] * 128);
log_key_val("m3", buffer[0][0] * 128);
}
// if all mics recorded a surge, do nothing (vibrations)
if (buffer[2][0] * 128 < mic_1_avg - current_threshold &&
buffer[1][0] * 128 < mic_2_avg - current_threshold &&
buffer[0][0] * 128 < mic_3_avg - current_threshold)
{
if (VERBOSE_MOTOR_SOFTWARE)
put_str_ln("Doing nothing");
motor_timer_init();
Reset_Motor_Instructions();
Reset_Mic_Procedure();
Add_Motor_Instruction(Motor_Control_Stop, 50);
Add_Motor_Instruction(Motor_Control_Stop, NEW_RECORD_DELAY);
Add_Motor_Instruction(Enable_ADC, 0);
Execute_Motor_Instructions();
}
// if right and left mic, go straight
else if (buffer[2][0] * 128 < mic_1_avg - current_threshold / 2 &&
buffer[0][0] * 128 < mic_3_avg - current_threshold / 2)
{
if (VERBOSE_MOTOR_SOFTWARE)
put_str_ln("Going straight");
if (MOVEMENT_ACTIVATED)
{
motor_timer_init();
Reset_Motor_Instructions();
Reset_Mic_Procedure();
Add_Motor_Instruction(Motor_Control_Forward, 300);
Add_Motor_Instruction(Motor_Control_Forward, 1000);
Add_Motor_Instruction(Motor_Control_Forward, 3000);
Add_Motor_Instruction(Motor_Control_Stop, 50);
Add_Motor_Instruction(Motor_Control_Stop, NEW_RECORD_DELAY);
Add_Motor_Instruction(Enable_ADC, 0);
Execute_Motor_Instructions();
}
}
// if left mic, turn left
else if(buffer[2][0] * 128 < mic_1_avg - current_threshold)
{
u8 ticks_ahead = 0;
u8 next_mic = Find_Next_Mic(0, &ticks_ahead, current_threshold / 2, 2);
u16 turn_delay = Calculate_Turn_Delay(next_mic, 0, 1, ticks_ahead);
if (VERBOSE_MOTOR_SOFTWARE)
{
put_str_ln("Going left");
log_key_val("next mic is", next_mic);
log_key_val("ticks ahead", ticks_ahead);
log_key_val("delay", turn_delay);
}
if (MOVEMENT_ACTIVATED)
{
motor_timer_init();
Reset_Motor_Instructions();
Reset_Mic_Procedure();
Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Turn_Left, turn_delay);
Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Turn_Right, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Left, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Right, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Left, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Right, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Left, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Stop, 50);
Add_Motor_Instruction(Motor_Control_Stop, NEW_RECORD_DELAY);
Add_Motor_Instruction(Enable_ADC, 0);
Execute_Motor_Instructions();
}
}
// if right mic, turn right
else if(buffer[0][0] * 128 < mic_3_avg - current_threshold)
{
u8 ticks_ahead = 0;
u8 next_mic = Find_Next_Mic(0, &ticks_ahead, current_threshold / 2, 0);
u16 turn_delay = Calculate_Turn_Delay(next_mic, 2, 1, ticks_ahead);
if (VERBOSE_MOTOR_SOFTWARE)
{
put_str_ln("Going right");
log_key_val("next mic is", next_mic);
log_key_val("ticks ahead", ticks_ahead);
log_key_val("delay", turn_delay);
}
if (MOVEMENT_ACTIVATED)
{
motor_timer_init();
Reset_Motor_Instructions();
Reset_Mic_Procedure();
Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Turn_Right, turn_delay);
Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Turn_Right, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Left, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Right, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Left, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Right, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Stop, 50);
Add_Motor_Instruction(Motor_Control_Stop, NEW_RECORD_DELAY);
Add_Motor_Instruction(Enable_ADC, 0);
Execute_Motor_Instructions();
}
}
// if back mic, turn around approx 140°
else if(buffer[1][0] * 128 < mic_2_avg - current_threshold)
{
u8 ticks_ahead = 0;
u8 next_mic = Find_Next_Mic(0, &ticks_ahead, current_threshold / 2, 1);
u16 turn_delay = Calculate_Back_Turn_Delay(ticks_ahead);
if (VERBOSE_MOTOR_SOFTWARE)
{
put_str_ln("Going back");
log_key_val("next mic is", next_mic);
log_key_val("ticks ahead", ticks_ahead);
log_key_val("delay", turn_delay);
}
if (MOVEMENT_ACTIVATED)
{
motor_timer_init();
Reset_Motor_Instructions();
Reset_Mic_Procedure();
Add_Motor_Instruction(Motor_Control_Backward, 50);
if (next_mic == 2)
Add_Motor_Instruction(Motor_Control_Turn_Right, turn_delay);
else
Add_Motor_Instruction(Motor_Control_Turn_Left, turn_delay);
/*Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Forward, 50);
Add_Motor_Instruction(Motor_Control_Turn_Left, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Right, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Left, SIDE_SWIM_DELAY);
Add_Motor_Instruction(Motor_Control_Turn_Right, SIDE_SWIM_DELAY);*/
Add_Motor_Instruction(Motor_Control_Stop, 50);
Add_Motor_Instruction(Motor_Control_Stop, NEW_RECORD_DELAY);
Add_Motor_Instruction(Enable_ADC, 0);
Execute_Motor_Instructions();
}
}
}
