//NOTE that the sensors read higher numbers when the object, or wall, are closer. (The farther, the lower number it is,)
int ldist; // LEFT SENSOR READ
int fdist; // FRONT SENSOR READ
int rdist; // RIGHT SENSOR READ
int fsense; // FLAME SENSOR READ
// PIN Assignments
//int switchPin = 12; // Pin for switch
int motor1Pin1 = 4; // left forwards (brown wire)
int motor1Pin2 = 5; // left backwards (white wire)
int motor2Pin1 = 3; // right forwards (red wire)
int motor2Pin2 = 2; // right backwards (black wire)
/
int groundPin = 8; // connect ground sensor to pin 8
int flamePin = 9; // connect flame sensor to pin 9
int fanPin = 10; // connect fan to pin 10
/
void loop() {
//currentState = digitalRead(switchPin); //What is the current state?
// currentState = HIGH; //This line is to test the code without the switch
//TURN ON: If the SWITCH is ON, then read sensors and move motors accordingly
if (currentState == HIGH)
{
digitalWrite(led, HIGH); // light turns on
ldist = analogRead(1);
fdist = analogRead(2);
rdist = analogRead(3);
//fsense = analogRead(4);
Serial.println();
Serial.println("New Readings and Actions");
Serial.print("Left"); Serial.print(" ");
Serial.print("Front"); Serial.print(" ");
Serial.print("Right"); Serial.print(" ");
//Serial.print("Flame Sensor Reads"); Serial.print(" ");
Serial.println();
Serial.print(ldist); Serial.print(" ");
Serial.print(fdist); Serial.print(" ");
Serial.print(rdist); Serial.print(" ");
//Serial.print(fsense); Serial.print(" ");
Serial.println();
//return; //Test sensors
// README:
// 1) Turning right is prioritized.
// If not right, go straight. If not right nor straight, turn left.
//DEFAULT: GO FORWARD
if ( (rdist > 200) && (fdist < 200) ) // if the rdist is close and front is open then go
{
Serial.println(" ");
Serial.print("GO FORWARD");
Serial.println(" ");
goForward();
}
//STOP
else if ( (fdist > 200) && (ldist > 200) && (rdist > 200) ) // but if fdist is too close, stop
{
Serial.println(" ");
Serial.print("STOP");
Serial.println(" ");
Stop();
}
//TURN RIGHT
if (rdist < 200) // if the right dist is far, then turn right
{
Serial.println(" ");
Serial.println("TURN RIGHT");
Serial.println(" ");
turnRight();
//goForward();
}
else if ( (ldist < 200) && (rdist > 200) && (fdist > 200) )
{
Serial.println(" ");
Serial.println("TURN LEFT");
Serial.println("");
turnLeft();
//goForward();
}
}
//TURN OFF the motors if the SWITCH is OFF
else if(currentState == LOW)
{
Serial.println("OFF");
Stop();
digitalWrite(led, LOW); // ADDED NOW
}
}
//****
//Defining Robot's Potential Actions as functions
//Stop, move forward, turn left, turn right
//Call different order of functions depending on the potential
//situations in the maze (3-way fork, left only, etc.)
//****
// Fire Fighting Robot Coding
//NOTE that the sensors read higher numbers when the object, or wall, are closer. (The farther, the lower number it is,) int ldist; // LEFT SENSOR READ int fdist; // FRONT SENSOR READ int rdist; // RIGHT SENSOR READ int fsense; // FLAME SENSOR READ
// PIN Assignments //int switchPin = 12; // Pin for switch int motor1Pin1 = 4; // left forwards (brown wire) int motor1Pin2 = 5; // left backwards (white wire) int motor2Pin1 = 3; // right forwards (red wire) int motor2Pin2 = 2; // right backwards (black wire) / int groundPin = 8; // connect ground sensor to pin 8 int flamePin = 9; // connect flame sensor to pin 9 int fanPin = 10; // connect fan to pin 10 /
int led = 11;
boolean currentState = HIGH;
void setup(){ // pinMode(switchPin, INPUT); pinMode(motor1Pin2, OUTPUT); pinMode(motor1Pin1, OUTPUT); pinMode(motor2Pin2, OUTPUT); pinMode(motor2Pin1, OUTPUT); //pinMode(fanPin, OUTPUT);
/ pinMode(groundPin, OUTPUT); pinMode(flamePin, OUTPUT); pinMode(fanPin, OUTPUT); /
pinMode(led, OUTPUT); Serial.begin(9600); /* while(digitalRead(switchPin) != HIGH) { analogWrite(led, LOW); } */ }
void loop() { //currentState = digitalRead(switchPin); //What is the current state? // currentState = HIGH; //This line is to test the code without the switch
//TURN ON: If the SWITCH is ON, then read sensors and move motors accordingly if (currentState == HIGH) { digitalWrite(led, HIGH); // light turns on
}
}
//**** //Defining Robot's Potential Actions as functions //Stop, move forward, turn left, turn right //Call different order of functions depending on the potential //situations in the maze (3-way fork, left only, etc.) //****
void Stop() { analogWrite(motor1Pin1, LOW); analogWrite(motor1Pin2, LOW); analogWrite(motor2Pin1, LOW); analogWrite(motor2Pin2, LOW);
}
void goForward() { analogWrite(motor1Pin1, 200); analogWrite(motor1Pin2, LOW); analogWrite(motor2Pin1, 200); analogWrite(motor2Pin2, LOW);
}
//SUGGESTION: //Turnining speed is slow
void turnRight() { analogWrite(motor1Pin1, 200); analogWrite(motor1Pin2, LOW); analogWrite(motor2Pin1, LOW); analogWrite(motor2Pin2, 200); }
//Turns 90 degrees counterclockwise, and continues forward
void turnLeft() { analogWrite(motor1Pin1, LOW); analogWrite(motor1Pin2, 200); analogWrite(motor2Pin1, 200); analogWrite(motor2Pin2, LOW);
}