Hi;
I am building and testing a telepresence wheeled rover bot that has two methods of control:
1. Web Interface (Manual Mode): This consists of a control panel on a web page served using WebIOPi on a Raspberry Pi which, when activated, sends commands to an Arduino Nano AT Mega328P via a two wire i2c interface (Forward, Back etc).
2. Autonomous Mode: If no data is received by the Nano over the i2c interface after a specified time frame the Nano switches to 'Autonomous Mode' and calls an auto navigate function. This causes the rover to drive around autonomously navigating its environment using an Ultrasonic Sensor (HC-SR04) mounted on a servo.
The wheels are driven by 2 x DC Motors connected to an L298N H Bridge.
The web interface was working fine and using Serial Debugging I could see that the Autonomous Mode was being called however the rover would not move.
After trying to debug my original code I decided to modify and upload a new program consisting of only autonomous ultrasonic obstacle avoidance. The purpose of this was to get a working auto navigating code working and then merge this with the manual control element.
Attached is my modified autonomous collision avoidance code, when it is run the ultrasonic servo centres and only turns one way, the robot then backs up and spins one way, it then just keeps repeating this movement cycle.
I have successfully used obstacle avoidance before using the Nano and HC-SR04 but in the most recent code attached I have tried to use the NewPing Library which I have never used before, according to the project that I magpied this code from an HC-SR04 was used?
I have also had to modify the veer left and veer right as my PWM is not working.
Could any of you kind folk take a look at my code and see if you can spot any errors that would prevent normal navigation? If the code is OK then i'll need to check my wiring and the sensor?
Many thanks
[code]/*MAEP 2.0 Navigation
by Noah Moroze, aka GeneralGeek
This code has been released under a Attribution-NonCommercial-ShareAlike license, more info at http://creativecommons.org/licenses/
PING))) code by David A. Mellis and Tom Igoe http://www.arduino.cc/en/Tutorial/Ping
*/
//Adapted by Bill Harvey for project Hammerstein
#include <Wire.h> //include i2c library
#include <Servo.h> //include Servo library
#define SLAVE_ADDRESS 0x04 //Set Arduino as slave and define the Serial bus address
//Setup Ultrasonic Sensor Pins
#define trigPin 13
#define echoPin 12
//Setup motor controller pins for L298N
//Right motor
int enA = 11;
int in1 = 8;
int in2 = 7;
//Left Motor
int enB = 10;
int in3 = 6;
int in4 = 5;
//const int RForward = 0;
//const int RBackward = 180;
//const int LForward = RBackward;
//const int LBackward = RForward;
//const int RNeutral = 90;
//const int LNeutral = 90; //constants for motor speed
//const int pingPin = 7;
//const int irPin = 0; //Sharp infrared sensor pin
const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance, rightDistance; //distances on either side
Servo sensor_servo; //Give servo a logical name
Servo leftMotor;
Servo rightMotor; //declare motors
long duration; //time it takes to recieve PING))) signal
long distance; //Added by Bill Harvey
void setup()
{
// rightMotor.attach(11);
// leftMotor.attach(10);
// sensor_servo.attach(6); //attach motors to proper pins
// sensor_servo.write(90); //set PING))) pan to center
delay(1000); // Sets a short delay after switching on
Serial.begin(9600); // Start serial for output debugging
Wire.begin(SLAVE_ADDRESS); // Initialise i2c as slave
// Wire.onReceive(receiveData); //Define callbacks for i2c communication
// Wire.onRequest(sendData); //Define callbacks for i2c communication
sensor_servo.attach(2); //Attaches the servo to pin 2 of the Nano
sensor_servo.write(90); //Set the servo to face front
//Set modes for distance sensor pins
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
//Set modes for motor controller pins
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
// Message to serial monitor declaring that robot is ready
Serial.println("Setup Complete");
Serial.println("Hammerstein Ready!");
}
void loop()
{
int distanceFwd = ping();
if (distanceFwd>dangerThresh) //if path is clear
{
Forward(); //move forward
}
else //if path is blocked
{
Stop();
sensor_servo.write(0);
delay(500);
rightDistance = ping(); //scan to the right
delay(500);
sensor_servo.write(180);
delay(700);
leftDistance = ping(); //scan to the left
delay(500);
sensor_servo.write(90); //return to center
delay(100);
compareDistance();
}
}
void compareDistance()
{
if (leftDistance>rightDistance) //if left is less obstructed
{
Left(); //turn left
delay(500);
}
else if (rightDistance>leftDistance) //if right is less obstructed
{
Right(); //turn right
delay(500);
}
else //if they are equally obstructed
{
Left(); //turn 180 degrees
delay(1000);
}
}
long ping()
{
// Send out PING))) signal pulse
//Trigger the sensor to send out a ping
digitalWrite(trigPin, HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) /29.1;
Serial.println(ping());
}
void Forward() //This function tells the robot to go forward
{
//for (int i=0; i <= 50; i++){
Serial.println("");
Serial.println("Moving forward");
// turn on left motor
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
// set speed out of possible range 0~255
analogWrite(enA, 255);
// turn on right motor
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
// set speed out of possible range 0~255
analogWrite(enB, 255);
delay(100);
//}
//Stop();
}
void Backward() //This function tells the robot to move backward
{
//for (int i=0; i <= 50; i++){
Serial.println("");
Serial.println("Moving backward");
// turn on left motor
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
// set speed out of possible range 0~255
analogWrite(enA, 255);
// turn on right motor
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
// set speed out of possible range 0~255
analogWrite(enB, 255);
delay(100);
//}
//Stop();
}
void Left() //This function tells the robot to turn left
{
//for (int i=0; i <= 50; i++){
Serial.println("");
Serial.println("Moving left");
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
// set speed out of possible range 0~255
analogWrite(enA, 240);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
//set speed out of possible range 0~255
analogWrite(enB, 255);
delay(100);
// }
// Stop();
}
void Right() //This function tells the robot to turn right
{
// for (int i=0; i <= 50; i++){
Serial.println("");
Serial.println("Moving right");
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
// set speed out of possible range 0~255
analogWrite(enA, 230);
digitalWrite(in3, HIGH); //Was High
digitalWrite(in4, LOW);
analogWrite(enB, 255);
delay(100);
// }
// Stop();
}
void Stop() //This function tells the robot to stop moving
{
Serial.println("");
Serial.println("Stopping");
// now turn off motors
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
//analogWrite(enA, 0);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
//analogWrite(enB, 0);
}
[/code]
