Afternoon all I have been working on this guy in my free time. (see attachment) Two HC-SR04 ultrasonic sensors and a seeed studio motor shield. I am posting for constructive criticism. He works pretty well need to adjust PWM to wheels for a better forward motion. The sketch is as follows,
#include <NewPing.h> //trig,echo,max distance
NewPing sonar1 (6, 5, 200);
NewPing sonar2 (4, 3, 200);
#define pingSpeed 100
unsigned long pingTimer1, pingTimer2;
int in1, in2;
int motorPin = 12; //right motor forward
int motorPin2 = 8; //left motor forward
int motorPin3 = 13; //right motor backward
int motorPin4 = 11; //left motor backward
//
void setup()
{
Serial.begin (9600);
pinMode(motorPin, OUTPUT); //motor outputs.
pinMode(motorPin2, OUTPUT);
pinMode(motorPin3, OUTPUT);
pinMode(motorPin4, OUTPUT);
pingTimer1 = millis() + pingSpeed; //fires sensor 1
pingTimer2 = pingTimer1 + 25; //fires sensor 2 25 ms later
}
int triggerDistance = 20;
int distance;
//int distance2;
int scan() {
if (millis() >= pingTimer1) {
pingTimer1 += pingSpeed; //sensor 1 fires again
in1 = sonar1.ping_cm();
if (in1 == 0) {
in1 = 100;
}
return in1;
}
if (millis() >= pingTimer2) {
pingTimer2 = pingTimer1 + 25; //sensor 2 fires 25 ms later
in2 = sonar2.ping_cm();
}
if (in2 == 0) {
in2 = 100;
}
return in2;
}
//function
void Forward()
{
digitalWrite(motorPin, HIGH);
digitalWrite(motorPin2, HIGH);
analogWrite(10, 150);
digitalWrite(motorPin3, LOW);
digitalWrite(motorPin4, LOW);
analogWrite(9, 140);
}
void Backward()
{
digitalWrite(motorPin, LOW);
digitalWrite(motorPin2, LOW);
analogWrite(10, 150);
digitalWrite(motorPin3, HIGH);
digitalWrite(motorPin4, HIGH);
analogWrite(9, 140);
}
void turnLeft()
{
digitalWrite(motorPin, HIGH);
digitalWrite(motorPin2, LOW);
analogWrite(10, 140);
digitalWrite(motorPin3, LOW);
digitalWrite(motorPin4, HIGH);
analogWrite(9, 140);
}
void turnRight()
{
digitalWrite(motorPin, LOW);
digitalWrite(motorPin2, HIGH);
analogWrite(10, 140);
digitalWrite(motorPin3, HIGH);
digitalWrite(motorPin4, LOW);
analogWrite(9, 140);
}
void moveStop()
{
digitalWrite(motorPin, LOW);
digitalWrite(motorPin2, LOW);
analogWrite(10, 190);
digitalWrite(motorPin3, LOW);
digitalWrite(motorPin4, LOW);
analogWrite(9, 190);
}
//
void loop()
{
(in1, in2 = scan());
if (in1 < triggerDistance || in2 < triggerDistance) {
Serial.print(in1);
Serial.print(" _ ");
Serial.println(in2);
moveStop();
Serial.println("Stop");
delay(500);
turnRight();
Serial.println("Right");
delay(350);
moveStop();
Serial.println("Stop");
delay(500);
(in1, in2 = scan());
Serial.print(in1);
Serial.print(" _ ");
Serial.println(in2);
if (in1 < triggerDistance || in2 < triggerDistance) {
turnLeft();
Serial.println("Left");
delay(700);
moveStop();
delay(500);
(in1, in2 = scan());
Serial.print(in1);
Serial.print(" _ ");
Serial.println(in2);
if (in1 < triggerDistance || in2 < triggerDistance) {
Backward();
delay(200);
Forward();
}
else {
Forward();
Serial.println("Forwards");
}
}
else {
Forward();
Serial.println("Forwards");
}
}
}
Thank you for any input.
