Project 6 4 in 1(line tracking, obstacle avoidance, IR control, bluetooth control) program
Code 9
#include <IRremote.h>
#include <Servo.h>
//*********************** define motor pin *************************
int MotorRight1=5;
int MotorRight2=6;
int MotorLeft1=10;
int MotorLeft2=11;
int counter=0;
const int irReceiverPin = 2; // set pin 2 as IR receiver signal OUTPUT
char val;
//***********************set the IRcode from the test result*************************
long IRfront= 0x00FF629D; // code for going forward
long IRback=0x00FFA857; // going backward
long IRturnright=0x00FFC23D; // turn right
long IRturnleft= 0x00FF22DD; // turn left
long IRstop=0x00FF02FD; // stop
long IRcny70=0x00FF6897; // CNY70 aoto-movingmode
long IRAutorun=0x00FF9867; // ultrasonic aoto-movingmode
long IRturnsmallleft= 0x00FFB04F;
//************************* define pin CNY70************************************
const int SensorLeft = 7; // input pin for left sensor
const int SensorMiddle= 4 ; // input pin for middle sensor
const int SensorRight = 3; // input pin for right sensor
int SL; // left sensor status
int SM; // middle sensor status
int SR; // right sensor status
IRrecv irrecv(irReceiverPin); // set IRrecv to receive IR signal
decode_results results; // decode result will be put into the variable of the result in the
//************************* define pin for ultrasonic ******************************
int inputPin =13 ; // define ultrasonic signal receiving pin rx
int outputPin =12; // define ultrasonic signal sending pin'tx
int Fspeedd = 0; // distance upfront
int Rspeedd = 0; // distance on the right
int Lspeedd = 0; // distance on the left
int directionn = 0; // F=8 B=2 L=4 R=6
Servo myservo; // set myservo
int delay_time = 250; // settling time for the servo motor moving backwards
int Fgo = 8; // going forward
int Rgo = 6; // going right
int Lgo = 4;// going left
int Bgo = 2;// going backwards
//********************************************************************(SETUP)
void setup()
{
Serial.begin(9600);
pinMode(MotorRight1, OUTPUT); // pin 8 (PWM)
pinMode(MotorRight2, OUTPUT); // pin 9 (PWM)
pinMode(MotorLeft1, OUTPUT); // pin 10 (PWM)
pinMode(MotorLeft2, OUTPUT); // pin 11 (PWM)
irrecv.enableIRIn(); // start IR decoding
pinMode(SensorLeft, INPUT); //define left sensor
pinMode(SensorMiddle, INPUT);// define middle sensor
pinMode(SensorRight, INPUT); // define right sensor
digitalWrite(2,HIGH);
pinMode(inputPin, INPUT); // define receiving pin for ultrasonic signal
pinMode(outputPin, OUTPUT); // define sending pin for ultrasonic signal
myservo.attach(9); // set servo motor output as pin 5(PWM)
}
//******************************************************************(Void)
void advance(int a) // go forward
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,HIGH);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,HIGH);
delay(a * 100);
}
void right(int b) //turn right(1 wheel)
{
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,HIGH);
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
delay(b * 100);
}
void left(int c) // turn left(1 wheel)
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,HIGH);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
delay(c * 100);
}
void turnR(int d) // turn right(2 wheels)
{
digitalWrite(MotorRight1,HIGH);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,HIGH);
delay(d * 100);
}
void turnL(int e) // turn left (2 wheels)
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,HIGH);
digitalWrite(MotorLeft1,HIGH);
digitalWrite(MotorLeft2,LOW);
delay(e * 100);
}
void stopp(int f) // stop
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
delay(f * 100);
}
void back(int g) // go backwards
{
digitalWrite(MotorRight1,HIGH);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,HIGH);
digitalWrite(MotorLeft2,LOW);;
delay(g * 100);
}
void detection() // measure 3 angles(0.90.179)
{
int delay_time = 250; // settling time for the servo motor moving backwards
ask_pin_F(); // read the distance upfront
if(Fspeedd < 10) // if distance less than 10cm
{
stopp(1); // clear output information
back(2); // oing backwards for 0.2second
}
if(Fspeedd < 15) // if distance less than 25cm
{
stopp(1); // clear output information
ask_pin_L(); // read the distance on the left
delay(delay_time); // settling time for the servo
ask_pin_R(); // read the distance on the right
delay(delay_time); // settling time for the servo
if(Lspeedd > Rspeedd) //if distance on the left is more than that on the right
{
directionn = Lgo; // go left
}
if(Lspeedd <= Rspeedd) // if distance on the left is less than that on the right
{
directionn = Rgo; // going right
}
if (Lspeedd < 10 && Rspeedd < 10) // if both distance are less than 10cm
{
directionn = Bgo; // going backwards
}
}
else // if the distance upfront is more than 25cm
{
directionn = Fgo; // going forward
}
}
//*****************************************************************************
void ask_pin_F() // measure the distance upfront
{
myservo.write(90);
delay(delay_time);
digitalWrite(outputPin, LOW);// ultrasonic sends out low voltage 2μs
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // ultrasonic sends out high voltage 10μs, at least 10μs
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // maintain low voltage sending
float Fdistance = pulseIn(inputPin, HIGH); // read the time difference
Fdistance= Fdistance/5.8/10; // convert time into distance (unit: cm)
Serial.print("Fdistance:"); //output distance in cm
Serial.println(Fdistance);// display distance
Fspeedd = Fdistance; // read the distance data into Fspeedd
}
//*****************************************************************************
void ask_pin_L() // measure the distance on the left
{
myservo.write(177);
delay(delay_time);
digitalWrite(outputPin, LOW); // ultrasonic sends out low voltage 2μs
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // ultrasonic sends out high voltage 10μs, at least 10μs
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // maintain low voltage sending
float Ldistance = pulseIn(inputPin, HIGH); // read the time difference
Ldistance= Ldistance/5.8/10; //converttime intodistance(unit: cm)
Serial.print("Ldistance:"); //output distance in cm
Serial.println(Ldistance);//display distance
Lspeedd = Ldistance; // read the distance data into Lspeedd
}
//*****************************************************************************
void ask_pin_R() // measure the distance on the right
{
myservo.write(5);
delay(delay_time);
digitalWrite(outputPin, LOW);// ultrasonic sends out low voltage 2μs
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // ultrasonic sends out high voltage 10μs, at least 10μs
delayMicroseconds(10);
digitalWrite(outputPin, LOW); //maintain low voltage sending
float Rdistance = pulseIn(inputPin, HIGH); //read the time difference
Rdistance= Rdistance/5.8/10; //convert time into distance(unit: cm)
Serial.print("Rdistance:"); //output distance in cm
Serial.println(Rdistance);//display distance
Rspeedd = Rdistance; // read the distance data into Rspeedd
}
//******************************************************************************(LOOP)
void loop()
{
SL=digitalRead(SensorLeft);
SM = digitalRead(SensorMiddle);
SR = digitalRead(SensorRight);
performCommand();
if(irrecv.decode(&results))
{
// finish decoding, receive IR signal
if (results.value == IRfront)// go forward
{
advance(10);// go forward
}
if (results.value == IRback)// go backward
{
back(5);// go backward
}
if (results.value == IRturnright)// turn right
{
right(5); // turn right
}
if (results.value == IRturnleft)// turn left
{
left(5); // turn left);
}
if (results.value == IRstop)// stop
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
}
if (results.value == IRcny70)
{
while(IRcny70)
{
SL= digitalRead(SensorLeft);
SM = digitalRead(SensorMiddle);
SR = digitalRead(SensorRight);
if (SM == HIGH)// middle sensor in black area
{
if (SL == LOW & SR == HIGH) // black on left, white on right, turn left
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,HIGH);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
}
else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
{
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,HIGH);
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
}
else // white on both sides, going forward
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,HIGH);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,HIGH);
}
}
else // middle sensor on white area
{
if (SL== LOW & SR == HIGH)// black on left, white on right, turn left
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,HIGH);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
}
else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
{
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,HIGH);
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
}
else // all white, stop
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
}
}
if(irrecv.decode(&results))
{
irrecv.resume();
Serial.println(results.value,HEX);
if(results.value==IRstop)
{
digitalWrite(MotorRight1,HIGH);
digitalWrite(MotorRight2,HIGH);
digitalWrite(MotorLeft1,HIGH);
digitalWrite(MotorLeft2,HIGH);
break;
}
}
}
results.value=0;
}
if (results.value ==IRAutorun )
{
while(IRAutorun)
{
myservo.write(90); // reset the servo motor and prepare it for the next measurement
detection(); // measure the angle and decide which direction to move
if(directionn == 8) // if directionn = 8
{
if(irrecv.decode(&results))
{
irrecv.resume();
Serial.println(results.value,HEX);
if(results.value==IRstop)
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
break;
}
}
results.value=0; advance(1); // going forward
Serial.print("Advance "); // display direction(forward)
Serial.print(" ");
}
if(directionn == 2) // if directionn = 2
{
if(irrecv.decode(&results))
{
irrecv.resume();
Serial.println(results.value,HEX);
if(results.value==IRstop)
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
break;
}
}
results.value=0;
back(2); // going backwards
turnL(3); // slightly move to the left to avoid stuck in the dead end
Serial.print(" Reverse "); // display direction (backwards)
}
if(directionn == 6) // if direction = 6
{
if(irrecv.decode(&results))
{
irrecv.resume();
Serial.println(results.value,HEX);
if(results.value==IRstop)
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
break;
}
}
results.value=0;
back(2);
turnR(3); // turn right
Serial.print(" Right "); // display direction(right)
}
if(directionn == 4) // if direction = 4
{
if(irrecv.decode(&results))
{
irrecv.resume();
Serial.println(results.value,HEX);
if(results.value==IRstop)
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
break;
}
}
results.value=0;
back(2);
turnL(3); // turn left
Serial.print(" Left "); // display direction(left)
}
if(irrecv.decode(&results))
{
irrecv.resume();
Serial.println(results.value,HEX);
if(results.value==IRstop)
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
break;
}
}
}
results.value=0;
}
else
{
digitalWrite(MotorRight1,LOW);
digitalWrite(MotorRight2,LOW);
digitalWrite(MotorLeft1,LOW);
digitalWrite(MotorLeft2,LOW);
}
irrecv.resume(); // continue receiving IR signal coming next
}
}
void performCommand()
{
if(Serial.available())
{
val = Serial.read();
}
if (val == 'U')
{
// Forward
advance(1);
}
else if (val == 'D')
{
//Backward
back(1);
}
else if (val == 'R')
{
// Right
turnR(1);
}
else if (val == 'L')
{
// Left
turnL(1);
}
else
if (val== 'S')
{
// Stop
stopp(1) ;
}
}