Hi everyone!
I'm working on a project which includes three ultrasonic sensors. I am using arduino Mega.
I connected all three ECO pins through an OR gate in order to mesure the ECHO pulse width with INT2 external interrupt. With OC0, OC1 and OC2, I am generating a 10us TRIG pulse, that way I know whos sensor pulse I measuring. I use TIMER5 as my counter to measure the pulse ON time.
It works greats for me with the sensor triggered by OC1B. when I try to use the two other sensors I get garbage values on my serial monitor:
(�␘���@aXa�␂��@aXa␘␘H␂@@@aXaH␂��@a�aL).
Even in case it is a hardware problem, I should read some unreliable value. But since it is a garbage values I guess it has something to do with my code.
I will mention the the calculation are excecuted and done by the ISR(INT2_vect). SInce that I suspect two things:
- I did not set up the timers correctly.
- Using Serial.println() is trying to use the same timers I use.
- I use wrong type of data for my numberws.
Here is my code and functions (pay attention to the function trigState() on line 71 in the main() function):
#include <Arduino.h>
#define DISABLE 0
#define ENABLE 1
// Define ultrasonic trigger pins
#define F_TRIG_PIN 12 // Ultrasoniv trigger pin, OCR1B
#define L_TRIG_PIN 9 // Ultrasoniv trigger pin, OCR2B
#define R_TRIG_PIN 4 // Ultrasoniv trigger pin, OCR0B
// Define motor control pins
#define RIGHT_FORWARD 2
#define RIGHT_BACKWARD 3
#define LEFT_FORWARD 6
#define LEFT_BACKWARD 7
// Define motor speeds
#define DRIVE 0//0xAF // Full Speed
#define STOP 0x00
#define N 65536 // Maximum value for a 16-bit counter to count
#define D_MIN 20 // Minimum front distance to stop [cm]
#define D_REV 10 // Front distance to drive backwards [cm]
#define TIMR1_PS 8 // IF CHAMGING - CHAMGE ALSO BITS IN TCCR1B REGISTER
///////////////////////////////////////////////////////////////////////////////////////
// Declare functions
void trigState(bool leftState, bool frontState, bool rightState); // Choose ultrasonic direction
void setupFrontUltrasonic();
void setupLeftUltrasonic();
void setupRightUltrasonic();
void enableEcho(); // Enable echo pulse capturing and measurement
void setupMotors(); // Setup motors PWM parameters
void setupCounter();
///////////////////////////////////////////////////////////////////////////////////////
// Parameters for ulstasonic interrupts
volatile int OVFcounter = 0;
volatile unsigned int N1 = 0, N2 = 0, dN = 0;
volatile unsigned int dT = 0, D = 0;
volatile bool rising = 1;
///////////////////////////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(9600);
// Define pins data direction
pinMode(RIGHT_FORWARD, OUTPUT);
pinMode(RIGHT_BACKWARD, OUTPUT);
pinMode(LEFT_FORWARD, OUTPUT);
pinMode(LEFT_BACKWARD, OUTPUT);
pinMode(F_TRIG_PIN, OUTPUT);
pinMode(L_TRIG_PIN, OUTPUT);
pinMode(R_TRIG_PIN, OUTPUT);
pinMode(13, OUTPUT);
// Set initial conditions
digitalWrite(F_TRIG_PIN, LOW);
digitalWrite(L_TRIG_PIN, LOW);
digitalWrite(R_TRIG_PIN, LOW);
digitalWrite(13, LOW);
// Initialize system
setupFrontUltrasonic();
setupLeftUltrasonic();
setupRightUltrasonic();
trigState(DISABLE, ENABLE, DISABLE);
enableEcho();
setupMotors();
setupCounter();
sei(); // Enable all interrupts
}
///////////////////////////////////////////////////////////////////////////////////////
void loop() {
Serial.println(D);
if(D <= D_MIN){
if(D<D_REV){
OCR3B = DRIVE; // RIGHT_BACKWARD speed (MAX is 0xFF)
OCR3C = STOP; // RIGHT_FORWARD speed (MAX is 0xFF)
OCR4A = STOP; // LEFT_FORWARD speed (MAX is 0xFF)
OCR4B = DRIVE; // LEFT_BACKWARD speed (MAX is 0xFF)
} else{
OCR3B = STOP; // RIGHT_BACKWARD speed (MAX is 0xFF)
OCR3C = STOP; // RIGHT_FORWARD speed (MAX is 0xFF)
OCR4A = STOP; // LEFT_FORWARD speed (MAX is 0xFF)
OCR4B = STOP; // LEFT_BACKWARD speed (MAX is 0xFF)
}
} else {
OCR3B = STOP; // RIGHT_BACKWARD speed (MAX is 0xFF)
OCR3C = DRIVE; // RIGHT_FORWARD speed (MAX is 0xFF)
OCR4A = DRIVE; // LEFT_FORWARD speed (MAX is 0xFF)
OCR4B = STOP; // LEFT_BACKWARD speed (MAX is 0xFF)
}
}
///////////////////////////////////////////////////////////////////////////////////////
// End of ultrasonic trigger pulse. Enable external interrupt for echo pulse width measurement
ISR(TIMER1_COMPB_vect){
EIMSK |= 1<<INT2;
TCNT5 = 0;
}
// External interrupt for echo pulse width measurement
ISR(INT2_vect){
digitalWrite(13, 0);
if(rising){
N1 = TCNT5;
//Flip state to interrupt on rising edge
EICRA &= 0<<ISC10;
rising = 0;
} else{
N2 = TCNT5;
//Flip state to interrupt on falling edge
EICRA |= 1<<ISC20;
rising = 1;
// Calculation to span distance values (D) accross timer counts values (N):
// Pulse width is: dT = dN*TIMR1_PS/FREQ
// dX = v*dT --> D = dT*C/2 --> D = dN*TIMR1_PS*C/(FREQ*2)
// Hence: D=0 | dN=0 and D=557[cm] | dN=N=65536
dN = N2-N1;
D = 557*(double)dN/N; // Results in [cm]
}
}
ISR(TIMER3_COMPB_vect){} // RIGHT_BACKWARD
ISR(TIMER3_COMPC_vect){} // RIGHT_FORWARD
ISR(TIMER4_COMPA_vect){} // LEFT_FORWARD
ISR(TIMER4_COMPB_vect){} // LEFT_BACKWARD
void setupFrontUltrasonic(){
TCCR1A = 0;
TCCR1B = 0;
TCCR1C = 0;
TIMSK1 = 0;
// Fast mode PWM
TCCR1A |= 1<<WGM10;
TCCR1A |= 1<<WGM11;
TCCR1B |= 1<<WGM12;
TCCR1B |= 1<<WGM13;
TCCR1A |= 1<<COM1B1; // OC1B output fall on compare match -> Datasheets Pg. 154
OCR1A = 0xFFFF; // MAX value for for counter 1B
OCR1B = 20; // Number of counts to generate 10 microseconds p
TIMSK1 |= 1<<OCIE1B; // Enable TIMER1_COMPB_vect ISR
return;
}
void setupLeftUltrasonic(){
TCCR2A = 0;
TCCR2B = 0;
TIMSK2 = 0;
// Fast mode PWM
TCCR2A |= 1<<WGM20;
TCCR2A |= 1<<WGM20;
TCCR2B |= 1<<WGM22;
TCCR2A |= 1<<COM2B1; // OC1B output fall on compare match -> Datasheets Pg. 154
OCR2A = 0xFF; // MAX value for for counter 1B
OCR2B = 20; // Number of counts to generate 10 microseconds pulse
TIMSK2 |= 1<<OCIE2B; // Enable TIMER2_COMPB_vect ISR
return;
}
void setupRightUltrasonic(){
TCCR0A = 0;
TCCR0B = 0;
TIMSK0 = 0;
// Fast mode PWM
TCCR0A |= 1<<WGM00;
TCCR0A |= 1<<WGM01;
TCCR0B |= 1<<WGM02;
TCCR0A |= 1<<COM0B1; // OC1B output fall on compare match -> Datasheets Pg. 154
OCR0A = 0xFF; // MAX value for for counter 1B
OCR0B = 20; // Number of counts to generate 10 microseconds p
TIMSK0 |= 1<<OCIE0B; // Enable TIMER1_COMPB_vect ISR
return;
}
void trigState(bool leftState, bool frontState, bool rightState){
if(leftState) // ENABLE
TCCR2B |= 1<<CS21;
else //DISABLE
TCCR2B &= 0b11111000;
if(frontState) // ENABLE
TCCR1B |= 1<<CS11;
else //DISABLE
TCCR1B &= 0b11111000;
if(rightState) // ENABLE
TCCR0B |= 1<<CS01;
else //DISABLE
TCCR0B &= 0b11111000;
return;
}
void enableEcho(){
EICRA = 0;
EIMSK = 0;
// Datasheets Pg. 110
EICRA |= 1<<ISC20; // | 1<<ISC21; // Select edge type to interrupt
EICRA |= 1<<ISC21;
EIMSK |= 1<<INT2; // Enable external interrupt
return;
}
void setupMotors(){
// Setting right motor PWM
// Drive D2 & D3 HIGH when TCNT3 < OCR3x while up-counting
// Drive D2 & D3 LOW when TCNT3 > OCR3x while up-counting
//>>>>>>>> DATASHEETS PG. 154
// Set outputs to work in Fast PWM mode
// >>>>>>>> DATASHEETS PG. 145 <<<<<<<<
// Set timers to count without prescalar
// >>>>>>>> DATASHEETS PG. 156 <<<<<<<<
TCCR3A = 0b00101001; // Pin toggling mode (HIGH to LOW)
TCCR3B |= 0b00001001; // Prescalar and Fast PWM
TCCR3B &= 0b11111001;
TIMSK3 |= 0b00001100; // Enable On-Compare-Interrupt B&C
OCR3B = 0; // RIGHT_BACKWARD speed (MAX is 0xFF)
OCR3C = 0; // RIGHT_FORWARD speed (MAX is 0xFF)
// Setting right motor PWM
// Drive D6 & D7 HIGH when TCNT4 < OCR4x while up-counting
// Drive D6 & D7 LOW when TCNT4 > OCRx while up-counting
//>>>>>>>> DATASHEETS PG. 154
// Set outputs to work in Fast PWM mode
// >>>>>>>> DATASHEETS PG. 145 <<<<<<<<
// Set timers to count without prescalar
// >>>>>>>> DATASHEETS PG. 156 <<<<<<<<
TCCR4A = 0b10100001; // Pin toggling mode (HIGH to LOW)
TCCR4B |= 0b00001001; // Prescalar and Fast PWM
TCCR4B &= 0b11111001;
TIMSK4 |= 0b00000110; // Enable On-Compare-Interrupt B&C
OCR4A = 0; // LEFT_FORWARD speed (MAX is 0xFF)
OCR4B = 0; // LEFT_BACKWARD speed (MAX is 0xFF)
}
void setupCounter(){
TCCR5A = 0;
TCCR5B = 0;
TCCR5C = 0;
TCCR5B = 1<<CS51; // No prescalar for maximum accuracy
return;
}