So i finally got my redbot to use an ultrasonic sensor for navigation but i am having an issue.... it runs into the wall anyway. what is happening is that the ultrasonic sensor doesn't pick up the obstruction until after it has hit it. then it will look left and right to find the best scenario.
Not quite sure what i am doing wrong here... not enough samples or delays causing the issue?? Baud rate?
/***********************************************************************
Right and Left motor encoder inputs must be connected to:
=========================================================
3 - Right wheel encoder
A2 - Left wheel encoder
If your connection is to different pins, simply alter the RedBotEncoder
pins to match the pins that your encoder is connected to.
Servo connection:
=================
A0 - white wire
5V - red wire
GND - black wire
HC-SR04 Ultrasonic connection:
=============================
RedBot HC-SR04
A4 Trig
A5 Echo
5V VCC
GND GND
***********************************************************************/
// Include the libraries. We make a provision for using the Xbee header
// via software serial to report values, but that's not really used in
// the code anywhere.
#include <RedBot.h>
RedBotMotors motors;
#include <Servo.h>
#include <Ultrasonic.h>
// Pin connections for the HC-SR04 module
#define trigPin A4
#define echoPin A5
Ultrasonic ultrasonic(A4,A5);
const int RForward = 0;
const int RBackward = 180;
const int LForward = 180;
const int LBackward = 0;
const int RNeutral = 91;
const int LNeutral = 92; //constants for motor speed
const int TrigPin = 12;
const int EchoPin = 13; //Echo pin
const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance = 0;
int rightDistance = 0; //distances on either side
Servo HeadServo;
long duration; // for HC-SR04
void setup()
{
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
HeadServo.attach(A0); //attach servo to proper pins
HeadServo.write(90); //centre head servo
delay(1000);
}
void loop()
{
int distanceFwd = (ultrasonic.Ranging(CM));
Serial.begin( 9600 );
Serial.print( ultrasonic.Ranging(CM) );
Serial.println( "cm" );
delay(500);
if (distanceFwd>dangerThresh) //if path is clear
{
motors.leftMotor(150); // spin CCW
motors.rightMotor(150); // spin CCW
}
else //if path is blocked
{
motors.leftMotor(0); // spin CCW
motors.rightMotor(0); // spin CCW
HeadServo.write(0);
delay(500);
rightDistance = ultrasonic.Ranging(CM); //scan to the right
delay(500);
HeadServo.write(180);
delay(700);
leftDistance = ultrasonic.Ranging(CM); //scan to the left
delay(500);
HeadServo.write(90); //return to center
delay(100);
compareDistance();
}
}
void compareDistance()
{
if (leftDistance>rightDistance) //if left is less obstructed
{
turnLeft();
delay(2000);
}
else if (rightDistance>leftDistance) //if right is less obstructed
{
turnRight();
delay(2000);
}
else //if they are equally obstructed
{
turn180();
delay(2000);
}
}
long ping()
{
// Send out PING))) signal pulse
pinMode(TrigPin, OUTPUT);
digitalWrite(TrigPin, LOW);
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(5);
digitalWrite(TrigPin, LOW);
//Get duration it takes to receive echo
pinMode(TrigPin, INPUT);
duration = pulseIn(TrigPin, HIGH);
//Convert duration into distance
return duration / 29 / 2;
}
// reverse() function -- backs up at full power
void reverse()
{
motors.drive(-255);
delay(500);
motors.brake();
delay(100); // short delay to let robot fully stop
}
// turnRight() function -- turns RedBot to the Right
void turnRight()
{
motors.leftMotor(+150); // spin CCW
motors.rightMotor(-150); // spin CCW
delay(500);
motors.brake();
delay(100); // short delay to let robot fully stop
}
// turnRight() function -- turns RedBot to the Left
void turnLeft()
{
motors.leftMotor(-150); // spin CW
motors.rightMotor(+150); // spin CW
delay(500);
motors.brake();
delay(100); // short delay to let robot fully stop
}
void turn180()
{
motors.leftMotor(+150); // spin CW
motors.rightMotor(-150); // spin CW
delay(500);
motors.brake();
delay(100); // short delay to let robot fully stop
}