const int Rin = 52 , Cin = 46, Lin = 53, Bin = 43, Rout = 50, Cout = 47, Lout = 51, Bout = 44; long Rduration, Cduration, Lduration, Bduration, Rdistance, Cdistance, Ldistance, Bdistance; boolean debug = false; //Serial communication for debuging. Set to true for serial communication #define ENA 5 //enable A on pin 5 (needs to be a pwm pin) #define ENB 3 //enable B on pin 3 (needs to be a pwm pin) #define IN1 2 //IN1 on pin 2 conrtols one side of bridge A #define IN2 4 //IN2 on pin 4 controls other side of A #define IN3 6 //IN3 on pin 6 conrtols one side of bridge B #define IN4 7 //IN4 on pin 7 controls other side of B byte serialA; void setup() { Serial.begin(9600); pinMode(ENA, OUTPUT); pinMode(ENB, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT); pinMode(10, OUTPUT); pinMode(11, OUTPUT); pinMode(21, OUTPUT); pinMode(20, OUTPUT); pinMode(19, OUTPUT); pinMode(18, OUTPUT); digitalWrite(11, HIGH); digitalWrite(21, HIGH); digitalWrite(19, HIGH); delay(15); digitalWrite(11, LOW); digitalWrite(21, LOW); digitalWrite(19, LOW); if (debug) { Serial.begin(9600); } } void loop() { if(Serial.available() > 0) { serialA = Serial.read(); Serial.println(serialA); } if(serialA == 'f') { (follow()); } else if(serialA == 'a') { (avoid()); } else if(serialA == 's') { motorA(0, 30); motorB(0, 30); digitalWrite(19, LOW); digitalWrite(20, LOW); digitalWrite(21, LOW); } else if(serialA == 'i') { motorA(1, 60); motorB(1, 60); } else if(serialA == 'm') { motorA(2, 60); motorB(2, 60); } else if(serialA == 'j') { motorA(1, 33); motorB(2, 33); } else if(serialA == 'k') { motorA(2, 33); motorB(1, 33); } else if(serialA == '1') { motorA(1, 30); motorB(1, 55); } else if(serialA == '2') { motorA(1, 55); motorB(1, 25); } else if(serialA == '3') { motorA(2, 25); motorB(2, 55); } else if(serialA == '4') { motorA(2, 55); motorB(2, 25); } else if(serialA == 'l') { digitalWrite(19, HIGH); digitalWrite(20, HIGH); digitalWrite(21, HIGH); digitalWrite(18, HIGH); } else if(serialA == 'o') { digitalWrite(19, LOW); digitalWrite(20, LOW); digitalWrite(21, LOW); digitalWrite(18, LOW); } else if(serialA == 'p') { digitalWrite(11, HIGH); delay(500); digitalWrite(11, LOW); delay(500); } else { motorA(0, 30); motorB(0, 30); digitalWrite(19, LOW); digitalWrite(20, LOW); digitalWrite(21, LOW); digitalWrite(11, LOW); } if (debug) { Serial.print("Left: "); Serial.print(Ldistance); Serial.println(" cm"); Serial.print("Right: "); Serial.print(Rdistance); Serial.println(" cm"); Serial.print("Center: "); Serial.print(Cdistance); Serial.println(" cm"); Serial.print("Back: "); Serial.print(Bdistance); Serial.println(" cm"); Serial.print("Bluetooth:"); Serial.println(serialA); delay(500); } } void follow(){ radar(); if (Ldistance < 25 || Cdistance < 25 || Rdistance < 25 || Bdistance < 25) { if (Ldistance < 24) { motorA(1, 31); motorB(2, 31); digitalWrite(19, HIGH); } else if (Rdistance < 24) { motorA(2, 31); motorB(1, 31); digitalWrite(20, HIGH); } else if (Cdistance > 15) { motorA(1, 35); motorB(1, 35); digitalWrite(21, HIGH); } else if (Cdistance < 8) { motorA(2, 35); motorB(2, 35); digitalWrite(21, HIGH); } else if (Bdistance < 10 && Cdistance > 8) { motorA(1, 35); motorB(1, 35); digitalWrite(21, HIGH); } } else { motorA(0, 30); motorB(0, 30); digitalWrite(19, LOW); digitalWrite(20, LOW); digitalWrite(21, LOW); } } void avoid(){ radar(); if (Rdistance < 22 && Ldistance > 22) { digitalWrite(20, HIGH); motorA(0, 30); motorB(2, 40); } else if (Ldistance < 22 && Rdistance > 22) { digitalWrite(19, HIGH); motorA(2, 40); motorB(0, 30); } else if (Cdistance < 22) { digitalWrite(21, HIGH); motorA(2, 40); motorB(2, 40); } else if (Bdistance < 15 && Cdistance > 15) { digitalWrite(21, HIGH); motorA(1, 40); motorB(1, 40); } else { digitalWrite(19, LOW); digitalWrite(20, LOW); digitalWrite(21, LOW); motorA(1, 50); motorB(1, 50); } } void radar() { if (serialA == 'a' || 'f') { pinMode(Rout, OUTPUT); delayMicroseconds(7); digitalWrite(Rout, LOW); delayMicroseconds(2); digitalWrite(Rout, HIGH); delayMicroseconds(5); digitalWrite(Rout, LOW); Rduration = pulseIn(Rin, HIGH); pinMode(Cout, OUTPUT); delayMicroseconds(7); digitalWrite(Cout, LOW); delayMicroseconds(2); digitalWrite(Cout, HIGH); delayMicroseconds(5); digitalWrite(Cout, LOW); Cduration = pulseIn(Cin, HIGH); pinMode(Lout, OUTPUT); delayMicroseconds(7); digitalWrite(Lout, LOW); delayMicroseconds(2); digitalWrite(Lout, HIGH); delayMicroseconds(5); digitalWrite(Lout, LOW); Lduration = pulseIn(Lin, HIGH); pinMode(Bout, OUTPUT); delayMicroseconds(7); digitalWrite(Bout, LOW); delayMicroseconds(2); digitalWrite(Bout, HIGH); delayMicroseconds(5); digitalWrite(Bout, LOW); Bduration = pulseIn(Bin, HIGH); Ldistance = (Lduration/2) / 29.1; Rdistance = (Rduration/2) / 29.1; Cdistance = (Cduration/2) / 29.1; Bdistance = (Bduration/2) / 29.1; } else { digitalWrite(Rout, LOW); digitalWrite(Lout, LOW); digitalWrite(Cout, LOW); digitalWrite(Bout, LOW); } } //****************** Motor A control ******************* void motorA(int mode, int percent) { //change the percentage range of 0 -> 100 into the PWM //range of 0 -> 255 using the map function int duty = map(percent, 0, 100, 0, 255); switch(mode) { case 0: //disable/coast digitalWrite(ENA, LOW); //set enable low to disable A break; case 1: //turn clockwise //setting IN1 high connects motor lead 1 to +voltage digitalWrite(IN1, HIGH); //setting IN2 low connects motor lead 2 to ground digitalWrite(IN2, LOW); //use pwm to control motor speed through enable pin analogWrite(ENA, duty); break; case 2: //turn counter-clockwise //setting IN1 low connects motor lead 1 to ground digitalWrite(IN1, LOW); //setting IN2 high connects motor lead 2 to +voltage digitalWrite(IN2, HIGH); //use pwm to control motor speed through enable pin analogWrite(ENA, duty); break; case 3: //brake motor //setting IN1 low connects motor lead 1 to ground digitalWrite(IN1, LOW); //setting IN2 high connects motor lead 2 to ground digitalWrite(IN2, LOW); //use pwm to control motor braking power //through enable pin analogWrite(ENA, duty); break; } } //********************************************************** //****************** Motor B control ******************* void motorB(int mode, int percent) { //change the percentage range of 0 -> 100 into the PWM //range of 0 -> 255 using the map function int duty = map(percent, 0, 100, 0, 255); switch(mode) { case 0: //disable/coast digitalWrite(ENB, LOW); //set enable low to disable B break; case 1: //turn clockwise //setting IN3 high connects motor lead 1 to +voltage digitalWrite(IN3, HIGH); //setting IN4 low connects motor lead 2 to ground digitalWrite(IN4, LOW); //use pwm to control motor speed through enable pin analogWrite(ENB, duty); break; case 2: //turn counter-clockwise //setting IN3 low connects motor lead 1 to ground digitalWrite(IN3, LOW); //setting IN4 high connects motor lead 2 to +voltage digitalWrite(IN4, HIGH); //use pwm to control motor speed through enable pin analogWrite(ENB, duty); break; case 3: //brake motor //setting IN3 low connects motor lead 1 to ground digitalWrite(IN3, LOW); //setting IN4 high connects motor lead 2 to ground digitalWrite(IN4, LOW); //use pwm to control motor braking power //through enable pin analogWrite(ENB, duty); break; } } //**********************************************************