Still having trouble with this code... any help would be appreciated!!
#include <Adafruit_MotorShield.h>
#include <NewPing.h>
#include <Wire.h>
#define trigPin A4
#define echoPin A5
#define MAX_DISTANCE 200
#define MAX_SPEED 200 // speed of motors
#define MAX_SPEED_OFFSET 20
NewPing sonar(trigPin, echoPin, MAX_DISTANCE);
int minSafeDist = 11;
int centerDist, leftDist, rightDist, backDist;
long duration, inches, cm;
long distance;
//Adafruit_MotorShield motor1(1);
//Adafruit_MotorShield motor2(2);
//Adafruit_MotorShield motor3(3);
//Adafruit_MotorShield motor4(4);
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motor4 = AFMS.getMotor(4);
Adafruit_DCMotor *motor3 = AFMS.getMotor(3);
Adafruit_DCMotor *motor1 = AFMS.getMotor(2);
Adafruit_DCMotor *motor2 = AFMS.getMotor(1);
void setup() {
Serial.begin(9600);
Serial.println("Motors On");
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
AFMS.begin();
motor1->setSpeed(200);
motor2->setSpeed(200);
motor3->setSpeed(200);
motor4->setSpeed(200);
}
void AllStop()
{
motor1->run(RELEASE); // turns off motor 1
motor2->run(RELEASE); // turns off motor 2
motor3->run(RELEASE); // turns off motor 3
motor4->run(RELEASE); // turns off motor 4
}
void AllForward()
{
motor1->run(FORWARD);
motor2->run(FORWARD);
motor3->run(FORWARD);
motor4->run(FORWARD);
Serial.println("Going forward");
}
void turnRight()
{
motor1->run(BACKWARD);
motor2->run(BACKWARD);
motor3->run(FORWARD);
motor4->run(FORWARD);
Serial.println("Turning right");
}
void GoBack()
{
motor1->run(BACKWARD);
motor2->run(BACKWARD);
motor3->run(BACKWARD);
motor4->run(BACKWARD);
Serial.println("Going backward");
}
void turnLeft()
{
motor1->run(FORWARD);
motor2->run(FORWARD);
motor3->run(BACKWARD);
motor4->run(BACKWARD);
Serial.println("Turning left");
}
void LookAhead()
{
// Need something in here.
}
long microsecondsToInches(long duration)
{
// Need something here.
return inches;
}
long microsecondsToCentimeters(long duration)
{
// Need something here.
return cm;
}
void loop()
{
LookAhead();
Serial.print(inches);
Serial.println(" inches");
if(inches >= minSafeDist)
{
AllForward();
delay(110);
}else
{
AllStop();
}
if(rightDist > leftDist)
// long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(5);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
if (distance < 10)
{
motor.goForward();
delay(2000);
motor.stop();
delay(1000);
}
else
{
motor.turnRight();
delay(2000);
motor.stop();
delay(1000);
}
if(distance >= 200 || distance <= 0)
{
Serial.println("Out of range");
}
else
{
Serial.print(distance);
Serial.println(" cm");
}
delay(500);
Serial.println("forward");
motor1->run(FORWARD);
motor2->run(FORWARD);
motor3->run(FORWARD);
motor4->run(FORWARD);
}