我在我的机器人项目中使用了三个声纳传感器，它们似乎在运行几秒后冻结。我不认为这是任何硬件故障。任何人都可以检查下面的代码，并判断是否有任何错误？
/*********************************************************************
This is the header file for sonar sensors to check if there is any
object in a given range. For the pin diagram, please refer resource
files.
The main functions used here to check if there is any object are
1. check_left()
2. check_right()
3. check_front()
and they return a short value 1 if there is any object or 0 if there
are none. So in order to use it in code, just assign the method name
to a short variable and treat it as a boolean value 1 or 0.
*********************************************************************/
#ifndef Sonar
#define Sonar
#define HIGH 1
#define LOW 0
#define ON 1
#define OFF 0
#define INPUT 1
#define OUTPUT 0
#define Sonar1TRIG_TRIS TRISB.B1
#define Sonar1TRIG_LAT LATB.B1
#define Sonar1ECHO_TRIS TRISB.B0
#define Sonar1ECHO_PORT PORTB.B0
#define Sonar2TRIG_TRIS TRISB.B3
#define Sonar2TRIG_LAT LATB.B3
#define Sonar2ECHO_TRIS TRISB.B2
#define Sonar2ECHO_PORT PORTB.B2
#define Sonar3TRIG_TRIS TRISB.B5
#define Sonar3TRIG_LAT LATB.B5
#define Sonar3ECHO_TRIS TRISB.B4
#define Sonar3ECHO_PORT PORTB.B4
#define CM40 1250
#define CM10 1125
/********************* Initiating variables *************************/
short right_flag, left_flag, front_flag, i, j, k;
/***************** Initiate Sonar PORTC and PORTD *******************/
void init_sonar() {
// Sonar One
Sonar1ECHO_TRIS = INPUT;
// Sonar Two
Sonar2ECHO_TRIS = INPUT;
// Sonar Three
Sonar3ECHO_TRIS = INPUT;
// Sonar Trigger 1
Sonar1TRIG_TRIS = OUTPUT;
// Sonar Trigger 2
Sonar2TRIG_TRIS = OUTPUT;
// Sonar Trigger 3
Sonar3TRIG_TRIS = OUTPUT;
}
/*************** Generate a pulse to start sonar 1 ******************/
void Trigger_1() {
//TRIGGER HIGH
Sonar1TRIG_LAT = HIGH;
// Delay
Delay_us(15);
//TRIGGER LOW
Sonar1TRIG_LAT = LOW;
}
/*************** Generate a pulse to start sonar 2 ******************/
void Trigger_2() {
//TRIGGER HIGH
Sonar2TRIG_LAT = HIGH;
// Delay
Delay_us(15);
//TRIGGER LOW
Sonar2TRIG_LAT = LOW;
}
/*************** Generate a pulse to start sonar 3 ******************/
void Trigger_3() {
//TRIGGER HIGH
Sonar3TRIG_LAT = HIGH;
// Delay
Delay_us(15);
//TRIGGER LOW
Sonar3TRIG_LAT = LOW;
}
/******************* Check left using Sonar *************************/
short check_left() {
// Initiate Timer and variables
T1CON = 0b00010000;
left_flag = 0; i = 0;
while(i < 5) {
Delay_ms(1);
// Send a pulse
Trigger_2();
//Wait till ECHO sets it value to High
while (Sonar2ECHO_PORT == 0) {asm nop;}
// Now start the timer and wait for the falling edge
T1CON.TMR1ON = 0;
TMR1H = 0x00; TMR1L = 0x00;
T1CON.TMR1ON = 1;
while((TMR1H << 8 | TMR1L) < CM40) {
if(Sonar2ECHO_PORT == 1) {continue;}
else {
// There is something inside this range
left_flag++;
break;
}
}
i++;
}
if (left_flag > 2) {
return 1;
} else {
return 0;
}
}
/********************* Check right using Sonar **********************/
short check_right() {
// Initiate Timer and variables
T1CON = 0b00010000;
right_flag = 0; j = 0;
while(j < 5) {
Delay_ms(1);
// Send a pulse
Trigger_1();
//Wait till ECHO sets it value to High
while (Sonar1ECHO_PORT == 0) {asm nop;}
// Now start the timer and wait for the falling edge
T1CON.TMR1ON = 0;
TMR1H = 0x00; TMR1L = 0x00;
T1CON.TMR1ON = 1;
while((TMR1H << 8 | TMR1L) < CM40) {
if(Sonar1ECHO_PORT == 1) {continue;}
else {
// There is something inside this range
right_flag++;
break;
}
}
j++;
}
if (right_flag > 2) {
return 1;
} else {
return 0;
}
}
/********************** Check front using Sonar *********************/
short check_front() {
// Initiate Timer and variables
T1CON = 0b00010000;
front_flag = 0; k = 0;
while(k < 5) {
Delay_ms(1);
// Send a pulse
Trigger_3();
//Wait till ECHO sets it value to High
while (Sonar3ECHO_PORT == 0) {asm nop;}
// Now start the timer and wait for the falling edge
T1CON.TMR1ON = 0;
TMR1H = 0x00; TMR1L = 0x00;
T1CON.TMR1ON = 1;
while((TMR1H << 8 | TMR1L) < CM10) {
if(Sonar3ECHO_PORT == 1) {continue;}
else {
// There is something inside this range
front_flag++;
break;
}
}
k++;
}
if (front_flag > 2) {
return 1;
} else {
return 0;
}
}
#endif
#include "Sonar.h"
#include "LED.h"
#include "IR.h"
#include "Motor.h"
#define ON 1
#define OFF 0
// Initiate
void init_main(void);
char inter_value, return_value;
short Ll, Rr, Ff;
/****************************** Initializing Codes ****************************/
// Initiate main panels
void init_main() {
init_sensor();
init_motor();
init_sonar();
init_PWM();
init_led();
}
/************************************** MAIN **********************************/
void main() {
// Initiate panels
init_main();
while(ON) {
Rr = check_right();
Ff = check_front();
Ll = check_left();
if (Ff == 1) {
LED6 = ON;
LED7 = OFF;
}
else {
LED7 = ON;
LED6 = OFF;
}
if (Ll == 1) {
LED1 = ON;
LED2 = OFF;
}
else {
LED2 = ON;
LED1 = OFF;
}
if (Rr == 1) {
LED3 = ON;
LED4 = OFF;
}
else {
LED4 = ON;
LED3 = OFF;
}
}
}