Hi,
Fairly sure it is the way I am trying to write this code that is causing problems so if anyone can help that would be great.
Problem: Object avoidance using 2x Ultrasonic sensors (SRF05), trying to get a vehicle to slow and turn if an object is "detected" within a predefined limit. So far can only get it to turn left, it seems to kinda ignore input from the Ultrasonics and turn in that direction when the left one is below the distance limit (so figure some kind of interrupt is working?).
The end goal for the project is to track a fast moving object across a fairly open terrain so I do not want the buggy to have to stop and pan left then right to check for obstacles (as some other projects do using just one sensor), just run and alter its course accordingly.
#include <Servo.h>
#include <Wire.h>
#include <NewPing.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
Servo servo1; // Create servo object
Servo servo2;
#define TRIGGER_PIN1 12
#define ECHO_PIN1 11
#define TRIGGER_PIN2 9
#define ECHO_PIN2 8
#define MAX_DISTANCE 200 // Maximum distance for sensor to check ahead
NewPing sonar1(TRIGGER_PIN1, ECHO_PIN1, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
NewPing sonar2(TRIGGER_PIN2, ECHO_PIN2, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *MotorL = AFMS.getMotor(1); //DC motor to drive vehicle
Adafruit_DCMotor *MotorR = AFMS.getMotor(2); //DC motor to drive vehicle
const int distLimit = 10; //Limit for obstacles (in cm)
int pos = 0; // variable to store the servo position
int i = 255; //Max speed to call for motors
void setup()
{
Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
servo1.attach(9); // attaches the servo on pin 9 to the servo object
servo2.attach(10);
AFMS.begin(); // create with the default frequency 1.6KHz
MotorL->setSpeed(255); // Set the speed to 255 (max speed)
MotorL->run(FORWARD);
MotorL->run(BACKWARD);
MotorL->run(RELEASE);
MotorR->setSpeed(255); // Set the speed to 255 (max speed)
MotorR->run(FORWARD);// turn on motor
MotorR->run(BACKWARD);
MotorR->run(RELEASE);
}
void loop()
{
for(pos = 45; pos < 135; pos += 1) // Servo goes from 45 degrees to 135 degrees in 1* steps
{
servo1.write(pos);
servo2.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
for(pos = 135; pos>=46; pos-=1) // Servo goes from 135 degrees to 46 degrees
{
servo1.write(pos);
servo2.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
int dist_R = sonar1.ping_cm(); //Ping the Right sensor
int dist_L = sonar2.ping_cm(); //Ping the Left sensor
Serial.print("Ping1: ");
Serial.print(dist_R);
Serial.println("cm 1");
Serial.print("Ping2: "); // These lines print ping results to serial monitor
Serial.print(dist_L);
Serial.println("cm 2");
if (dist_R<distLimit)//if Right sensor returns a value less than limit
{
turnLeft();
}
else if (dist_L<distLimit)//if Left sensor returns a value less than limit
{
turnRight();
}
else
{
MotorL->run(FORWARD);//Run both motors forwards at max speed
MotorR->run(FORWARD);
MotorL->setSpeed(i);
MotorR->setSpeed(i);
}
}
void turnRight()//Command for right turns
{
MotorL->run(FORWARD);
MotorR->run(FORWARD);
MotorL->setSpeed(i);
MotorR->setSpeed(i-55);
}
void turnLeft()//Command for left turns
{
MotorL->run(FORWARD);
MotorR->run(FORWARD);
MotorL->setSpeed(i-55);
MotorR->setSpeed(i);
}