I am doing a project which concentrates on a bot/cart following a human using ultrasonic sensors, two motors and a free wheel.
I managed to get the distance between an ultrasonic transmitter and 2 ultrasonic receivers, credits to Mr.Mark on the codes he sent on this thread right here.
Having the data on the 2 sensors, what I did was subtract the distance between them and find the direction using the sign I acquired. (If it's positive bot goes right, otherwise left.) Next I did was to get the average distance to control speed.
Problem is, the motors doesn't do what I've coded. Can you spot what I'm missing? Here is my code.
Transmitter:
/*
Transmit side of one way ultrasonic ranging using HC-SR04.
Node sync is performed with 433 MHz OOK transmit receive pair via a sync sequence
HC-SR04 Ping distance sensor:
VCC to arduino 5v
GND to arduino GND
Echo to Arduino pin 7
Trig to Arduino pin 8
433 MHz Transmitter:
Data to Arduino pin 9
HC-SR04 technique from here: http://arduinobasics.blogspot.com.au/2012/11/arduinobasics-hc-sr04-ultrasonic-sensor.html
*/
#define echoPin 7 // Echo Pin
#define trigPin 8 // Trigger Pin
#define rfTxPin 9 // rf Transmit enable
#define LEDPin 13 // Onboard LED
#define BitPeriod 500 // Bit period for RF sync bits in microseconds
boolean syncSeq[16] = {1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 1} ; // Bit sequence for synchronization
int maximumRange = 2000; // Maximum range needed
int minimumRange = 0; // Minimum range needed
long duration, distance; // Duration used to calculate distance
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(rfTxPin, OUTPUT);
pinMode(LEDPin, OUTPUT); // Use LED indicator (if required)
digitalWrite(trigPin, LOW) ;
digitalWrite(rfTxPin, LOW) ;
}
void loop() {
digitalWrite(LEDPin, HIGH);
TxRFSync() ; // Send RF synchronization sequence
digitalWrite(trigPin, HIGH); // Start ping sequence
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
digitalWrite(LEDPin, LOW);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration / 58.2;
if (distance >= maximumRange || distance <= minimumRange) {
/* Send a negative number to computer */
Serial.println("-1");
}
else {
/* Send the distance to the computer using Serial protocol. */
Serial.println(distance);
}
//Delay 100ms before next reading.
delay(1);
}
// Function to write "syncSeq" bit sequence to RF transmit port
void TxRFSync()
{
for (int k = 0; k < sizeof(syncSeq) ; k++)
{
digitalWrite(rfTxPin, syncSeq[k]) ;
delayMicroseconds(BitPeriod) ;
}
digitalWrite(rfTxPin, LOW) ; // Turn off transmit at end of sequence
}
Receiver:
/*
Receive side of one way ultrasonic ranging using HC-SR04.
Node sync is performed with 433 MHz OOK transmit receive pair via a sync sequence
HC-SR04 Ping distance sensor:
VCC to arduino 5v
GND to arduino GND
Echo to Arduino pin 7
Trig to Arduino pin 8
433 MHz Receiver:
Data to Arduino pin 9
HC-SR04 scheme from here: http://arduinobasics.blogspot.com.au/2012/11/arduinobasics-hc-sr04-ultrasonic-sensor.html
*/
#define syncStateA 10
#define syncStateB 11
#define debugPinC 12
#define echoPin2 3 // Echo Pin2
#define trigPin2 4 // Trigger Pin2
#define echoPin 5 // Echo Pin
#define trigPin 6 // Trigger Pin
#define rfRxPin 7 // rf Receive input pin
#define LEDPin 2 // Onboard LED
#define Forward1 13 //Forward right wheel
#define Back1 12 //Backward right wheel
#define PWM1 11 //Control right
#define PWM2 10 //Control left
#define Forward2 9 //Forward left wheel
#define Back2 8 //Backward left wheel
// Parameters for sync detection
#define minBitPeriod 450 // Low threshold for bit period (uS)
#define maxBitPeriod 550 // Max threshold for bit period (uS)
#define minSyncBits 8 // Min number of valid 1 plus 0 transitions before 1 plus 1
int A;
int B;
int maximumRange = 2000; // Maximum range needed (cm)
int minimumRange = 0; // Minimum range needed
long duration, distance, duration2, distance2; // Duration used to calculate distance
long timeout = 50000; // Ping duration timeout
void setup() {
pinMode(syncStateA, OUTPUT) ;
pinMode(syncStateB, OUTPUT) ;
pinMode(debugPinC, OUTPUT) ;
Serial.begin (9600);
pinMode(Forward1, OUTPUT);
pinMode(Back1, OUTPUT);
pinMode(PWM1, OUTPUT);
pinMode(Forward2, OUTPUT);
pinMode(Back2, OUTPUT);
pinMode(PWM2, OUTPUT);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(rfRxPin, INPUT);
pinMode(LEDPin, OUTPUT) ;
digitalWrite(LEDPin, LOW) ;
digitalWrite(trigPin, LOW) ;
digitalWrite(trigPin2, LOW) ;
}
void loop() {
{
RxRFSync() ; // Function blocks until sync sequence is detected
digitalWrite(LEDPin,HIGH) ;
digitalWrite(trigPin, HIGH); // Start ping sequence
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH, timeout);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration / 29.1 ;
digitalWrite(LEDPin,LOW) ;
}
{
RxRFSync() ; // Function blocks until sync sequence is detected
digitalWrite(LEDPin,HIGH) ;
digitalWrite(trigPin2, HIGH); // Start ping sequence
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH, timeout);
//Calculate the distance (in cm) based on the speed of sound.
distance2 = duration2 / 29.1 ;
digitalWrite(LEDPin,LOW) ;
}
//start of computation, values are based on a straight line
//distance = first sensor left, distance2 = second sensor right
{
long A = distance - distance2; //I used this to determine what direction the bot will go, if values go positive goes left, otherwise go right
long B = ((distance + distance2)/2); //This is used for the control of speed, average distance
Serial.print("A = ");
Serial.println(A);
Serial.print("B = ");
Serial.println(B);
if (A >= -3 || A <= 3) //got this for error leeway
{
if (B >= 35 || B <= 58) //35cm is about 1ft and 58 is about 2ft
{
digitalWrite(Forward1, HIGH);
digitalWrite(Back1, LOW);
digitalWrite(Forward2, HIGH);
digitalWrite(Back2, LOW);
analogWrite(PWM1, 125);
analogWrite(PWM2, 125);
if (B < 35)
digitalWrite(Forward1, LOW);
digitalWrite(Back1, LOW);
digitalWrite(Forward2, LOW);
digitalWrite(Back2, LOW);
analogWrite(PWM1, 0);
analogWrite(PWM2, 0);
}
if (B > 58)
{
digitalWrite(Forward1, HIGH);
digitalWrite(Back1, LOW);
digitalWrite(Forward2, HIGH);
digitalWrite(Back2, LOW);
analogWrite(PWM1, 200);
analogWrite(PWM2, 200);
}
}
if (A < -3)
{
digitalWrite(Forward1, HIGH);
digitalWrite(Back1, LOW);
analogWrite(PWM1, 200);
digitalWrite(Forward2, LOW);
digitalWrite(Back2, LOW);
}
if (A > 3)
{
digitalWrite(Forward1, LOW);
digitalWrite(Back1, LOW);
digitalWrite(Forward2, HIGH);
digitalWrite(Back2, LOW);
analogWrite(PWM2, 200);
}
}
}
// Function to detect "syncSeq" bit sequence on RF receive port
void RxRFSync()
{
long int lastBitTime = 0 ; // holds time of last bit transition (uS)
int BitCount = 0 ; // counts number of valid bits detected
boolean synced = false ;
boolean lastBit = LOW ; // holds last bit detected
digitalWrite(syncStateA, LOW) ; digitalWrite(syncStateB, LOW) ; digitalWrite(debugPinC, LOW) ;
while (!synced)
{
while (digitalRead(rfRxPin) == lastBit) { } // Block until bit transition
int currBitTime = micros() ;
int bitPeriod = currBitTime - lastBitTime ;
if ((bitPeriod > (2 * minBitPeriod)) && (bitPeriod < (2 * maxBitPeriod)) && (lastBit == HIGH) && (BitCount > minSyncBits))
{
// Valid completion of sync sequence
synced = true ;
digitalWrite(syncStateA, HIGH) ; digitalWrite(syncStateB, HIGH) ; digitalWrite(debugPinC, lastBit) ;
}
else
{
lastBit = !lastBit ;
lastBitTime = currBitTime ;
if ((bitPeriod > minBitPeriod) && (bitPeriod < maxBitPeriod))
{
// Valid single bit detected, increment valid bit count and look for next bit
BitCount += 1 ; // increment valid bit count
digitalWrite(syncStateA, LOW) ; digitalWrite(syncStateB, HIGH) ; digitalWrite(debugPinC, lastBit) ;
}
else
{
// Invalid bit detected, reset valid bit count and look for next bit
BitCount = 0 ;
digitalWrite(syncStateA, HIGH) ; digitalWrite(syncStateB, LOW) ; digitalWrite(debugPinC, lastBit) ;
}
}
}
}
EDIT: I'm also using a l298n driver for this project. Thanks in advance for your help!
