#define TRIG1 PIN_B3
#define ECHO1 PIN_B5
#define TRIG2 PIN_B2
#define ECHO2 PIN_B4
#define ULTRASONIC_START() output_high(TRIG1);delay_us(15);output_low(TRIG1)
void rb_isr(){
echo1_act = input(ECHO1);
if(echo1_act && !echo1_last)
set_timer3(0);
else if(!echo1_act && echo1_last){
time1 = get_timer3();
echo1_ready=1;
}
echo1_last = echo1_act;
}
void init(){
// Digital IO
set_tris_a(0b111111); //Y_RATE,RA4,Vref+,Z_ACC,Y_ACC,X_ACC
set_tris_b(0b11110011); //PGD,PGC,ECHO1,ECHO2,TRIG1,TRIG2,RB1,RB0
set_tris_c(0b11111111); //RC7,RC6,D+,D-,VUSB,PWM1,PWM2,RC0
set_tris_d(0b11000000); //RD7,SWITCH,LED_R,LED_G,1A,2A,3A,4A
set_tris_e(0b111); //RE2,RE1,X_RATE
port_b_pullups(TRUE);
// Timer3
setup_timer_3 ( T3_INTERNAL | T3_DIV_BY_8 );
// Interrupts
disable_interrupts(INT_RB);
enable_interrupts(GLOBAL);
}
void main(){
init();
set_motors(0,0);
output_low(TRIG1);
enable_interrupts(INT_RB);
enable_interrupts(INT_TIMER1);
ULTRASONIC_START();
while(TRUE){
if(echo1_ready){
echo1_ready=0;
dist1=(float)time1*0.011494252;
ULTRASONIC_START();
}
}
if(!(dist1>4 && dist1<10))
set_motors(0,0);
else
set_motors(FORWARD_GAIN*pitch + STEERING_GAIN*roll, FORWARD_GAIN*pitch - STEERING_GAIN*roll);
}
}