#define leftp 7
#define leftn 4
#define rightp 3
#define rightn 2
#define speedleft 5
#define speedright 6
#define surfaceSense1 9
#define surfaceSense2 8
//Function prototypes
void straight(int speedpwm);
void left(int speedpwm);
void right(int speedpwm);
void reverse(int speedpwm);
void brake(int speedpwm);
void readSurface();
void setup() {
Serial.begin(9600);
pinMode(leftp, OUTPUT);
pinMode(leftn, OUTPUT);
pinMode(rightp, OUTPUT);
pinMode(rightn, OUTPUT);
pinMode(speedleft, OUTPUT);
pinMode(speedright, OUTPUT);
pinMode(surfaceSense1, INPUT);
pinMode(surfaceSense2, INPUT);
void straight(int speedpwm){
digitalWrite(leftp, HIGH);
digitalWrite(leftn, LOW);
digitalWrite(rightp, HIGH);
digitalWrite(rightn, LOW);
analogWrite(speedleft, speedpwm);
analogWrite(speedright, speedpwm);
}
void left(int speedpwm){
digitalWrite(leftp, LOW);
digitalWrite(leftn, LOW);
digitalWrite(rightp, HIGH);
digitalWrite(rightn, LOW);
analogWrite(speedleft, speedpwm);
analogWrite(speedright, speedpwm);
}
void right(int speedpwm){
digitalWrite(leftp, HIGH);
digitalWrite(leftn, LOW);
digitalWrite(rightp, LOW);
digitalWrite(rightn, LOW);
analogWrite(speedleft, speedpwm);
analogWrite(speedright, speedpwm);
}
void reverse(int speedpwm){
digitalWrite(leftp, LOW);
digitalWrite(leftn, HIGH);
digitalWrite(rightp, LOW);
digitalWrite(rightn, HIGH);
analogWrite(speedleft, speedpwm);
analogWrite(speedright, speedpwm);
}
void brake(int speedpwm){
digitalWrite(leftp, LOW);
digitalWrite(leftn, LOW);
digitalWrite(rightp, LOW);
digitalWrite(rightn, LOW);
analogWrite(speedleft, speedpwm);
analogWrite(speedright, speedpwm);
}
void readSurface(){
//For left sensor
if(digitalRead(surfaceSense1) == HIGH){
//Check if both sensor was triggered simultaneously
if(digitalRead(surfaceSense2) == HIGH){
//Robot backs up for a bit before making appropriate turning
reverse(255);
delay(1000);
}
//Here the robot avoids the line from the left
right(255);
delay(500);
//Keeps executing until the line was cleared from sensor
readSurface();
}
//For right sensor
else if(digitalRead(surfaceSense2) == HIGH){
//Check if both sensor was triggered simultaneously
if(digitalRead(surfaceSense2) == HIGH){
//Robot backs up for a bit before making appropriate turning
reverse(255);
delay(1000);
}
//Here the robot avoids the line from the right
left(255);
delay(500);
//Keeps executing until the line was cleared from sensor
readSurface();
}
Comments