#include <Servo.h>
int pinLB = 12; // define pin 12
int pinLF = 3; // define pin 3
int pinRB = 13; // define pin 13
int pinRF = 11; // define pin 11
int inputPin = 4; // define pin for sensor echo
int outputPin =5; // define pin for sensor trig
int Fspeedd = 0; // forward speed
int Rspeedd = 0; // right speed
int Lspeedd = 0; // left speed
int directionn = 0; // forward=8 backward=2 left=4 right=6
Servo myservo; // set myservo
int delay_time = 250; // settling time after steering servo motor moving B
int Fgo = 8; // Move F
int Rgo = 6; // move to the R
int Lgo = 4; // move to the L
int Bgo = 2; // move B
int Bluetooth_val; //
void setup()
{
Serial.begin(9600); // Define motor output pin
pinMode(pinLB,OUTPUT); // pin 12
pinMode(pinLF,OUTPUT); // pin 3 (PWM)
pinMode(pinRB,OUTPUT); // pin 13
pinMode(pinRF,OUTPUT); // pin 11 (PWM)
pinMode(inputPin, INPUT); // define input pin for sensor
pinMode(outputPin, OUTPUT); // define output pin for sensor
myservo.attach(9); // Define servo motor output pin to D9 (PWM)
}
void advance() // move forward
{
digitalWrite(pinLB,LOW); // right wheel moves forward
digitalWrite(pinRB, LOW); // left wheel moves forward
analogWrite(pinLF,255);
analogWrite(pinRF,255);
}
void stopp() // stop
{
digitalWrite(pinLB,HIGH);
digitalWrite(pinRB,HIGH);
analogWrite(pinLF,0);
analogWrite(pinRF,0);
}
void right() // turn right (single wheel)
{
digitalWrite(pinLB,HIGH); // wheel on the left moves forward
digitalWrite(pinRB,LOW); // wheel on the right moves backward
analogWrite(pinLF, 255);
analogWrite(pinRF,255);
}
void left() // turn left (single wheel)
{
digitalWrite(pinLB,LOW); // wheel on the left moves backward
digitalWrite(pinRB,HIGH); // wheel on the right moves forward
analogWrite(pinLF, 255);
analogWrite(pinRF,255);
}
void back() // move backward
{
digitalWrite(pinLB,HIGH); // motor moves to left rear
digitalWrite(pinRB,HIGH); // motor moves to right rear
analogWrite(pinLF,255);
analogWrite(pinRF,255);
}
void detection() // measure 3 angles (0.90.179)
{
int delay_time = 250; // stabilizing time for servo motor after moving backward
ask_pin_F(); // read the distance ahead
if(Fspeedd < 10) // if distance ahead is <10cm
{
stopp(); // clear data
delay(100);
back(); // move backward for 0.2S
delay(200);
}
if(Fspeedd < 25) // if distance ahead is <25cm
{
stopp();
delay(100); // clear data
ask_pin_L(); // read distance on the left
delay(delay_time); // stabilizing time for servo motor
ask_pin_R(); // read distance on the right
delay(delay_time); // stabilizing time for servo motor
if(Lspeedd > Rspeedd) // if distance on the left is >distance on the right
{
directionn = Lgo; // move to the L
}
if(Lspeedd <= Rspeedd) // if distance on the left is <= distance on the right
{
directionn = Rgo; // move to the right
}
if (Lspeedd < 10 && Rspeedd < 10) // if distance on left and right are both <10cm
{
directionn = Bgo; // move backward
}
}
else // if distance ahead is >25cm
{
directionn = Fgo; // move forward
}
}
void ask_pin_F() // measure the distance ahead
{
myservo.write(90);
digitalWrite(outputPin, LOW); // ultrasonic sensor transmit low level signal 2渭s
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // ultrasonic sensor transmit high level signal10渭s, at least 10渭s
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // keep transmitting low level signal
float Fdistance = pulseIn(inputPin, HIGH); // read the time in between
Fdistance= Fdistance/5.8/10; // convert time into distance (unit: cm)
Fspeedd = Fdistance; // read the distance into Fspeedd
Serial.print("Fspeedd = ");
Serial.print(Fspeedd );
Serial.println(" cm"); delay(1);
}
void ask_pin_L() // measure distance on the left
{
myservo.write(5);
delay(delay_time);
digitalWrite(outputPin, LOW); // ultrasonic sensor transmit low level signal 2渭s
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // ultrasonic sensor transmit high level signal10渭s, at least 10渭s
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // keep transmitting low level signal
float Ldistance = pulseIn(inputPin, HIGH); // read the time in between
Ldistance= Ldistance/5.8/10; // convert time into distance (unit: cm)
Lspeedd = Ldistance; // read the distance into Lspeedd
Serial.print("Lspeedd = ");
Serial.print(Lspeedd );
Serial.print(" cm ");delay(1);
}
void ask_pin_R() // measure distance on the right
{
myservo.write(177);
delay(delay_time);
digitalWrite(outputPin, LOW); // ultrasonic sensor transmit low level signal 2渭s
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // ultrasonic sensor transmit high level signal10渭s, at least 10渭s
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // keep transmitting low level signal
float Rdistance = pulseIn(inputPin, HIGH); // read the time in between
Rdistance= Rdistance/5.8/10; // convert time into distance (unit: cm)
Rspeedd = Rdistance; // read the distance into Rspeedd
Serial.print(" Rspeedd = ");
Serial.print(Rspeedd );
Serial.println(" cm");
delay(1);
}
//通过蓝牙发送2进入超声波模块，当蓝牙输入E时，退出超声波模式，并且小车停止
void Ultrasonic_car()
{
while(Bluetooth_val != 'E')
{
Bluetooth_val = Serial.read();
myservo.write(90); // home set the servo motor, ready for next measurement
detection(); // measure the angle and determine which direction to move
if(directionn == 2) // if directionn= 2
{
back();
delay(800); // go backward
left() ;
delay(200); // Move slightly to the left (to prevent stuck in dead end)
}
if(directionn == 6) // if directionn = 6
{
back();
delay(100);
right();
delay(600); // turn right
}
if(directionn == 4) // if directionn = 4
{
back();
delay(600);
left();
delay(600); // turn left
}
if(directionn == 8) // if directionn = 8
{
advance(); // move forward
delay(100);
}
}
stopp();
}
//通过蓝牙发送1进入蓝牙模块，当蓝牙输入E时，退出蓝牙模式，并且小车停止
void Bluetooth_car()
{
while(Bluetooth_val != 'E')
{
if (Serial.available())
{
Bluetooth_val = Serial.read();
if(Bluetooth_val == 'U')advance();
if(Bluetooth_val == 'D')back();
if(Bluetooth_val == 'L')left() ;
if(Bluetooth_val == 'R')right();
if(Bluetooth_val == 'S')stopp();
}
}
stopp();
}
void loop()
{
if (Serial.available())
{
Bluetooth_val = Serial.read();
if(Bluetooth_val == '1') Bluetooth_car(); //当蓝牙输入E时，退出蓝牙模式
if(Bluetooth_val == '2') Ultrasonic_car(); //当蓝牙输入E时，退出超声波模式
}
}
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
