I am trying to build an object avoidance car for my project in class. The car has the HC-SR04 mounted on a SG90. This portion works. I also have the L298N as the h-bridge and it works. Where i am having trouble is the compare statement in the programming. Any advice would be greatly appreciated.
#include<Servo.h>
Servo myservo;
const int Dir1PinA = 6; // direction motor A
const int Dir2PinA = 7; // direction motor A
const int Dir1PinB = 9; // direction motor B
const int Dir2PinB = 10; // direction motor B
const int speedPinA = 5; // pwm motor A
const int speedPinB = 11; // pwm motor B
int pos = 0;
int photocellPin = A0;// Photocell connected to analog pin 0
int photocellVal = 0; // define photocell variable
int ledPin = 13;// LED connected to digital pin 9
int ledState = 0;//state of the led
int fadeDown = 30;//delay per fade
int fadeUp = 20;//delay per fade
int minLight = 700;//min light threshold
int maxLight = 700;//max light threshold
int leftDistance, rightDistance, fwdDistance;
const int trigPin = 2;
const int echoPin = 3;
long duration, inches, in, cm;
int yes, no;
void setup()
{
Serial.begin(9600);
pinMode(photocellPin, INPUT);
pinMode(ledPin, OUTPUT);
myservo.attach(12);
myservo.write(100);
delay(1500);
}
void loop()
{
photocellVal = analogRead(photocellPin);
if (photocellVal < minLight and ledState == 0) {
fadeLed(1);
}
else if (photocellVal > maxLight and ledState == 1){
fadeLed(0);
}
leftPing();
fwdPing();
rightPing();
compareDistance();
}
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
void fadeLed (int num) {
if (num == 1) {
for (int fadeValue = 0 ; fadeValue <= 255; fadeValue += 5) {
analogWrite(ledPin, fadeValue);
delay(fadeUp);
}
ledState = 1;
}
else {
for (int fadeValue = 255 ; fadeValue >= 0; fadeValue -= 5) {
analogWrite(ledPin, fadeValue);
delay(fadeDown);
}
ledState = 0;
}
}
void compareDistance()
{
if (leftDistance = no, fwdDistance = yes, rightDistance = yes) {
turnleft();
delay(800);
}
else if (leftDistance = yes, fwdDistance = yes, rightDistance = no) {
turnright();
delay(800);
}
else if (leftDistance = no, fwdDistance = no, rightDistance = no) {
forward();
delay(1500);
}
else if ( leftDistance = yes, fwdDistance = yes, rightDistance = yes) {
reverse();
delay(800);
}
}
void leftPing()
{
for (pos = 100; pos >= 50; pos -= 1) {
myservo.write(pos);
delay(15);
}
delay(600);
getDistance();
if(inches>10)
{
leftDistance = no;
}
else(inches<10);
{
leftDistance = yes;
}
}
void fwdPing(){
for (pos = 50; pos < 100; pos += 1)
{ // in steps of 1 degree
myservo.write(pos);
delay(15);
}
delay(600);
getDistance();
if(inches>10)
{
fwdDistance = no;
}
else(inches<10);
{
fwdDistance = yes;
}
}
void rightPing(){
for (pos = 100; pos < 150; pos += 1)
{ // in steps of 1 degree
myservo.write(pos);
delay(15);
}
delay(600);
getDistance();
if(inches>10){
rightDistance = no;
}
else(inches<10);
{
rightDistance = yes;}
for (pos = 150; pos >= 100; pos -= 1)
{
myservo.write(pos);
delay(15);
}
}
void getDistance()
{
pinMode(trigPin, OUTPUT);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
Serial.print(inches);
Serial.print("in, ");
Serial.print(cm);
Serial.print("cm");
Serial.println();
delay(100);
}
void turnleft()
{
digitalWrite(Dir1PinA, LOW);
digitalWrite(Dir2PinA, HIGH);
digitalWrite(Dir1PinB, LOW);
digitalWrite(Dir2PinB, HIGH);
analogWrite(speedPinA, 130);
analogWrite(speedPinB, 130);
}
void turnright()
{
digitalWrite(Dir1PinA, HIGH);
digitalWrite(Dir2PinA, LOW);
digitalWrite(Dir1PinB, LOW);
digitalWrite(Dir2PinB, HIGH);
analogWrite(speedPinA, 130);
analogWrite(speedPinB, 130);
}
void forward()
{
digitalWrite(Dir1PinA, LOW);
digitalWrite(Dir2PinA, LOW);
digitalWrite(Dir1PinB, LOW);
digitalWrite(Dir2PinB, HIGH);
analogWrite(speedPinA, 70);
analogWrite(speedPinB, 70);
}
void reverse()
{
digitalWrite(Dir1PinA, LOW);
digitalWrite(Dir2PinA, LOW);
digitalWrite(Dir1PinB, HIGH);
digitalWrite(Dir2PinB, LOW);
analogWrite(speedPinA, 70);
analogWrite(speedPinB, 70);
}
void stop()
{
analogWrite(speedPinA, 0);
analogWrite(speedPinB, 0);
}