Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //******************************
- #include <IRremote.h>
- #include <Servo.h>
- gt
- //*********************** 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();
- //***************************************************normal remote control mode
- 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);
- }
- //****************************************************** black and white line mode
- 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;
- }
- //******************************************************** ultrasonic auto-moving mode
- 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) ;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement