#define in1 9
#define in2 8
#define in3 7
#define in4 6
#define enA 10
#define enB 5
int M1_Speed = 120; // speed of motor 1
int M2_Speed = 120; // speed of motor 2
int LeftRotationSpeed = 250; // Left Rotation Speed
int RightRotationSpeed = 250; // Right Rotation Speed
void setup() {
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(A1, INPUT); // initialize Left sensor as an input
pinMode(A0, INPUT); // initialize Right sensor as an input
Serial.begin(9600);
//analogWrite(enA, M1_Speed); // Write The Duty Cycle 0 to 255 Enable Pin A for Motor1 Speed
//analogWrite(enB, M2_Speed); // Write The Duty Cycle 0 to 255 Enable Pin B for Motor2 Speed
delay(1000);
}
void loop() {
int LEFT_SENSOR = digitalRead(A0);
int RIGHT_SENSOR = digitalRead(A1);
if (RIGHT_SENSOR == 0 && LEFT_SENSOR == 0) {
forward(); //FORWARD
Serial.print("l:");
Serial.println(LEFT_SENSOR);
Serial.print("r:");
Serial.println(RIGHT_SENSOR);
Serial.println("direction:forward");
// delay(1000);
}
else if (RIGHT_SENSOR == 1 && LEFT_SENSOR == 0) {
right(); //Move Right
Serial.print("l:");
Serial.println(LEFT_SENSOR);
Serial.print("r:");
Serial.println(RIGHT_SENSOR);
Serial.println("direction:r");
// delay(1000);
}
else if (RIGHT_SENSOR == 0 && LEFT_SENSOR == 1) {
left(); //Move Left
Serial.print("l:");
Serial.println(LEFT_SENSOR);
Serial.print("r:");
Serial.println(RIGHT_SENSOR);
Serial.println("direction:l");
// delay(1000);
}
else if (RIGHT_SENSOR == 1 && LEFT_SENSOR == 1) {
Stop(); //STOP
Serial.print("l:");
Serial.println(LEFT_SENSOR);
Serial.print("r:");
Serial.println(RIGHT_SENSOR);
Serial.println("direction:s");
// delay(1000);
}
}
void forward()
{
digitalWrite(in1, LOW); //Right Motor forword Pin
digitalWrite(in2, HIGH); //Right Motor backword Pin
digitalWrite(in3, HIGH); //Left Motor backword Pin
digitalWrite(in4, LOW); //Left Motor forword Pin
analogWrite(enA, M1_Speed);
analogWrite(enB, M2_Speed);
}
void backward()
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, M1_Speed);
analogWrite(enB, M2_Speed);
}
void right()
{
digitalWrite(in1, LOW); //Right Motor forword Pin
digitalWrite(in2, HIGH); //Right Motor backword Pin
digitalWrite(in3, LOW); //Left Motor backword Pin
digitalWrite(in4, HIGH); //Left Motor forword Pin
analogWrite(enA, LeftRotationSpeed);
analogWrite(enB, RightRotationSpeed);
}
void left()
{
digitalWrite(in1, HIGH); //Right Motor forword Pin
digitalWrite(in2, LOW); //Right Motor backword Pin
digitalWrite(in3, HIGH); //Left Motor backword Pin
digitalWrite(in4, LOW); //Left Motor forword Pin
analogWrite(enA, LeftRotationSpeed);
analogWrite(enB, RightRotationSpeed);
}
void Stop()
{
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
0 Comments