Hi everyone! I am trying to make a robot with two motors and two sensors. If the sensor hits the wall it should go the opposite direction that it is currently going in. However, the motors don't stop even when I trigger the sensors. Can someone help? Thanks so much!
int M1dirP = 4; //motor 1 direction
int M1spdP = 5; //motor 1 speed
int trigPin = 4; //switch in front connected to pin 4
int echoPin =5; //switch in back connected to pin 5
long distance;
long duration;
int spd1 = 150;
int FWD1 = HIGH;
int BKWD1 = !FWD1;
int M2dirP = 6; //motor2
int M2spdP = 7; //motor 2
int FWD2 = HIGH;
int BKWD2 = !FWD2;
int spd2 = 250; //speed = 0-255
void setup() {
Serial.begin(9600);
pinMode(trigPin, OUTPUT);//
pinMode(echoPin, INPUT);//
pinMode(M1dirP, OUTPUT);
pinMode(M1spdP, OUTPUT);
pinMode(M2dirP, OUTPUT);
pinMode(M2spdP, OUTPUT);
}
void loop() {
//for ( spd1 = 0; spd1 <spd2; spd1+10)
//{
digitalWrite(M2dirP, FWD2); //wheel = motor 2
analogWrite(M2spdP, spd2);
digitalWrite(M1dirP, FWD1); //wheel = motor 1
analogWrite(M1spdP, spd1); //analog write
digitalWrite (trigPin, LOW);
delayMicroseconds(2);
digitalWrite (trigPin, HIGH);
delayMicroseconds (10);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2)/29.1;
if (digitalRead(trigPin == HIGH) && digitalRead(echoPin == LOW)) //if the sensor hits the fall in front switch reads either 0 or 1
{
digitalWrite(M1dirP, BKWD1);
digitalWrite(M2dirP, BKWD2);
Serial.println("Stop");
}
else //if (digitalRead(trigPin == LOW) && digitalRead(echoPin == HIGH))//
{
digitalWrite(M1dirP, FWD1);
digitalWrite(M2dirP, FWD2);
Serial.println("Keep going");
}
if (digitalRead(echoPin == HIGH) && digitalRead(trigPin ==LOW)) //if sensor hits the back
{
digitalWrite(M1dirP, FWD1);
digitalWrite(M2dirP, FWD2);
Serial.println("Stop");
}
else //if(digitalRead(echoPin == LOW) && digitalRead(echoPin == HIGH))
{
digitalWrite(M1dirP, BKWD1);
digitalWrite(M2dirP, BKWD2);
Serial.println("Keep going");
}
delay(250);
}
