#define IN1 pin_A1
#define IN2 pin_A2
#define IN3 pin_A3
#define IN4 pin_A4
#define SRF05_TRIGGER1 PIN_B1
#define SRF05_ECHO PIN_B0
#define NO_OBJECT 0
unsigned int8 value;
unsigned int32 num_pulse=0;
int1 range_ok=0;
int i=0;
short modo_manual=0, modo_automatico=0;
void manual(void);
void automatico(void);
#INT_EXT
void Int_externa(void)
{
disable_interrupts(GLOBAL);
num_pulse+=get_timer1();
range_ok=1;
enable_interrupts(GLOBAL);
}
#INT_TIMER1
void Temp_Timer1(void)
{
disable_interrupts(GLOBAL);
num_pulse+=0xffff;
enable_interrupts(GLOBAL);
}
void SRF05_StartRange()
{
while(!range_ok)
{
output_high(SRF05_TRIGGER1);
delay_ms(15); // Crea un pulso de al menos 10 ms de duración
output_low(SRF05_TRIGGER1); // Iniciar la medición.
while(!(input(SRF05_ECHO))); // Espere hasta que el pin ECHO esté alto
set_timer1(0);
enable_interrupts(GLOBAL);
delay_ms(50);
}
}
float32 SRF05_GetDistance(int16 cao, int16 thap)
{
for( i=0;i<=50;i++)
{
output_high(PIN_A0);
delay_us (cao);
output_low(PIN_A0);
delay_us (thap);
}
float32 time_us=0,distance=0;
SRF05_StartRange();
disable_interrupts(GLOBAL);
if(num_pulse>150000)
{
num_pulse=0;
range_ok=0;
return NO_OBJECT;
}
else
{
time_us=num_pulse/5;
distance=time_us/58;
num_pulse=0;
range_ok=0;
return distance;
}
}
void automatico()
{
output_high(IN1);
output_low(IN2);
output_high(IN3);
output_low(IN4); // Avanzar / tien
float32 front = SRF05_GetDistance(1500,1500); // 0
float32 left, righ;
if (front >= 40)
{
output_high(IN1);
output_low(IN2);
output_high(IN3);
output_low(IN4); // Avanzar / tien
}
if( front < 40 && front > 0)
{
output_low(IN1);
output_low(IN2);
output_low(IN3);
output_low(IN4); // Detenido
delay_ms(300);
output_low(IN1);
output_high(IN2);
output_low(IN3);
output_high(IN4); // Atras
delay_ms(100);
output_low(IN1);
output_low(IN2);
output_low(IN3);
output_low(IN4); // Detenido
delay_ms(100);
left = SRF05_GetDistance(2000,1000);
delay_ms(600);
righ = SRF05_GetDistance(1000,2000);
delay_ms(600);
if(left > righ)
{
output_low(IN1);
output_high(IN2);
output_high(IN3);
output_low(IN4); // Va a la izquierda / trai
delay_ms(300); // thoi gian robo quay goc 90 //290
output_low(IN1);
output_low(IN2);
output_low(IN3);
output_low(IN4); // Detenido
delay_ms(100);
}
else
{
output_high(IN1);
output_low(IN2);
output_low(IN3);
output_high(IN4);
delay_ms(300);// thoi gian robo quay goc 90
output_low(IN1);
output_low(IN2);
output_low(IN3);
output_low(IN4); // Detenido
delay_ms(100);
}
}
}
#int_rda
void ngat(void)
{
value = getch();
}
void manual()
{
if(value == 'F'){ output_high(IN1);
output_low(IN2);
output_high(IN3);
output_low(IN4); } // Avanzar / tien
if(value == 'B'){ output_low(IN1);
output_high(IN2);
output_low(IN3);
output_high(IN4); } // Retroceder / lui
if(value == 'L'){ output_low(IN1);
output_high(IN2);
output_high(IN3);
output_low(IN4); } // Va a la izquierda / trai
if(value == 'R'){ output_high(IN1);
output_low(IN2);
output_low(IN3);
output_high(IN4); } // Va a la derecha / phai
if(value == 'G'){ output_low(IN1);
output_low(IN2);
output_high(IN3);
output_low(IN4); } // Avanzar a la izquierda / tien_trai
if(value == 'I'){ output_high(IN1);
output_low(IN2);
output_low(IN3);
output_low(IN4); } // Avanzar a la derecha / tien_phai
if(value == 'H'){ output_low(IN1);
output_low(IN2);
output_low(IN3);
output_high(IN4); } // trasera a la izquierda / lui_trai
if(value == 'J'){ output_low(IN1);
output_high(IN2);
output_low(IN3);
output_low(IN4); } // trasera a la derecha / lui_phai
if(value == 'S'){ output_low(IN1);
output_low(IN2);
output_low(IN3);
output_low(IN4); } // Detenido
if(value == '0'){ set_pwm1_duty((int16)0); set_pwm2_duty((int16)0); }
if(value == '4'){ set_pwm1_duty((int16)400); set_pwm2_duty((int16)400); }
if(value == '5'){ set_pwm1_duty((int16)500); set_pwm2_duty((int16)500); }
if(value == '6'){ set_pwm1_duty((int16)600); set_pwm2_duty((int16)600); }
if(value == '7'){ set_pwm1_duty((int16)700); set_pwm2_duty((int16)700); }
if(value == '8'){ set_pwm1_duty((int16)800); set_pwm2_duty((int16)800); }
if(value == '9'){ set_pwm1_duty((int16)900); set_pwm2_duty((int16)900); }
if(value == 'q'){ set_pwm1_duty((int16)1023); set_pwm2_duty((int16)1023); }
}
void main()
{
porta=0x00; set_tris_a(0b00000000); portb=0x00; set_tris_b(0b00000001); portc=0x00; set_tris_c(0b10000000);
output_float(SRF05_ECHO);
output_drive(SRF05_TRIGGER1);
ext_int_edge(H_TO_L); // ngat canh xuong
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);//F_TIMER1=F_OSC/4
enable_interrupts(INT_TIMER1);
enable_interrupts(INT_EXT); // kich hoat
setup_ccp1(CCP_PWM);
setup_ccp2(CCP_PWM);
setup_timer_2(T2_DIV_BY_16,255,1); // 819 us overflow, 819 us interrupt
enable_interrupts(global);
enable_interrupts(int_rda);
output_low(IN1);
output_low(IN2);
output_low(IN3);
output_low(IN4);
while(TRUE)
{
if(value == 'A'){ enable_interrupts(INT_TIMER1); enable_interrupts(INT_EXT); automatico(); }
if(value == 'M'){disable_interrupts(INT_TIMER1); disable_interrupts(INT_EXT); manual(); }
}
}