Scribd
Upload
62 views

Programa-Arduino-kit-montaje Robot PDF

Uploaded by

harold ramirez
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
62 views

Programa-Arduino-kit-montaje Robot PDF

Uploaded by

harold ramirez
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 4

PROGRAMACIÓN DEL COCHE ULTRASONIDOS HC

/*
L=left
R=right
F=forward
B=backward

*/
#include <Servo.h>

int pinLB=6; // define 6 pin left backward int pinLF=9; // define 9


pin left forward int pinRB=3; // define 10 pin right backward int
pinRF=5; // define 11 pin right forward int inputPin = A0; //
define ultrasonic signal receiving pin int outputPin = A1; // define
ultrasonic signal transmission pin
int Fspeedd = 0; // forward speed int
Rspeedd = 0; // turning right speed int
Lspeedd = 0; // turning left speed
int directionn = 0; // forward=8 backward=2 left=4 right=6 Servo
myservo; // set myservo
int delay_time = 250; // stabilizing time of servo motor after turning int
Fgo = 8; // forward int Rgo = 6; // turn right int Lgo = 4; // turn left int
Bgo = 2; // backward

void setup()
{
Serial.begin(9600); // define motor output pin
pinMode(pinLB,OUTPUT); // pin 8 (PWM)
pinMode(pinLF,OUTPUT); // pin 9 (PWM)
pinMode(pinRB,OUTPUT); // pin 10 (PWM)
pinMode(pinRF,OUTPUT); // pin 11 (PWM) pinMode(inputPin,
INPUT); // define ultrasonic input pin pinMode(outputPin,
OUTPUT); // define ultrasonic output pin myservo.attach(11); //
define the fifth pin of servo motor(PWM)
}

void advance(int a) // forward


{
digitalWrite(pinRB,LOW); // let motor act(right back) digitalWrite(pinRF,HIGH);
digitalWrite(pinLB,LOW); // let motor act(left back)
digitalWrite(pinLF,HIGH); delay(a * 100); }
void right(int b) //turn right(single wheel)
{
digitalWrite(pinRB,LOW); //let motor act(right back)
digitalWrite(pinRF,HIGH);
digitalWrite(pinLB,HIGH);
digitalWrite(pinLF,HIGH); delay(b
* 100);
}

void left(int c) //turn left(single wheel)


{ digitalWrite(pinRB,HIGH);
digitalWrite(pinRF,HIGH); digitalWrite(pinLB,LOW);
//let motor act(left back) digitalWrite(pinLF,HIGH);
delay(c * 100); }

void turnR(int d) //turn right(double wheel)


{
digitalWrite(pinRB,LOW); //let motor act(right back)
digitalWrite(pinRF,HIGH); digitalWrite(pinLB,HIGH);
digitalWrite(pinLF,LOW); //let motor act(left front)
delay(d * 100); }

void turnL(int e) //turn left(double wheel)


{ digitalWrite(pinRB,HIGH); digitalWrite(pinRF,LOW);
//let motor act(right front) digitalWrite(pinLB,LOW);
//let motor act(left back) digitalWrite(pinLF,HIGH);
delay(e * 100); }

void stopp(int f) //stop


{ digitalWrite(pinRB,HIGH);
digitalWrite(pinRF,HIGH);
digitalWrite(pinLB,HIGH);
digitalWrite(pinLF,HIGH);
delay(f * 100); }

void back(int g) //backward


{
digitalWrite(pinRB,HIGH); //let motor act(right back)
digitalWrite(pinRF,LOW); digitalWrite(pinLB,HIGH);
//let motor act(left back) digitalWrite(pinLF,LOW);
delay(g * 100); }

void ask_pin_F() // measure distance ahead


{
myservo.write(90);
digitalWrite(outputPin, LOW); // let ultrasonic transmit low voltage 2μs delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // let ultrasonic transmit hight voltage 10μs, at least 10μs here
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // keep ultrasonic transmitting low voltage
float Fdistance = pulseIn(inputPin, HIGH); // read time gap Fdistance=
Fdistance/5.8/10; // convert time into distance(unit:cm)

20
Serial.print(“F distance:”); //output distance(unit:cm)
Serial.println(Fdistance); //display distance
Fspeedd = Fdistance; // read distance into Fspeedd(forward speed)
}

void ask_pin_L() // measure distance on the left side


{ myservo.write(5);
delay(delay_time);
digitalWrite(outputPin, LOW); // let ultrasonic transmit low voltage 2μs delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // let ultrasonic transmit hight voltage 10μs, at least 10μs here
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // keep ultrasonic transmitting low voltage
float Ldistance = pulseIn(inputPin, HIGH); // read time gap Ldistance=
Ldistance/5.8/10; // convert time into distance(unit:cm)
Serial.print(“L distance:”); //output distance(unit:cm)
Serial.println(Ldistance); //display distance
Lspeedd = Ldistance; // read distance into Lspeedd(Leftward speed)
}

void ask_pin_R() // measure distance on the right side


{
myservo.write(177); delay(delay_time);
digitalWrite(outputPin, LOW); // let ultrasonic transmit low voltage 2μs delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // let ultrasonic transmit high voltage 10μs, at least 10μs here delayMicroseconds(10);
digitalWrite(outputPin, LOW); // keep ultrasonic transmitting low voltage
float Rdistance = pulseIn(inputPin, HIGH); // read time gap Rdistance=
Rdistance/5.8/10; // convert time into distance(unit:cm)
Serial.print(“R distance:”); //output distance(unit:cm)
Serial.println(Rdistance); //display distance
Rspeedd = Rdistance; // read distance into Rspeedd(rightward speed)
}

void detection() //measure three angles(0.90.179)


{
int delay_time = 250; // stabilizing time of servo motor after turning ask_pin_F(); //
read distance ahead
if(Fspeedd < 10) // if distance ahead is less than 10cm
{
stopp(1); // Clear output data back(2);
// backward for 0.2 sec
}
if(Fspeedd < 25) // if distance ahead is less than 25cm
{
stopp(1); // Clear output data ask_pin_L(); // read
distance on the left side delay(delay_time); // wait
for the servo to stabilize ask_pin_R(); // ead
distance on the right side delay(delay_time); // wait
for the servo to stabilize
if(Lspeedd > Rspeedd) //if the distance on the left side is larger than that of the right side
{
directionn = Rgo; //go rightwards
}
if(Lspeedd <= Rspeedd) //if the distance on the left side is no more than that of the right side
{
directionn = Lgo; //go leftwards
}
if (Lspeedd < 10 && Rspeedd < 10) //if the distance on both sides is less than 10cm
{
directionn = Bgo; //go backwards

}}
else //if the distance ahead if no less than 25cm
{
directionn = Fgo; //go forward
}}

void loop() {
myservo.write(90); //let servo motor return to its ready position, prepared for the next measurement
detection(); //measure angle and decide moving direction if(directionn == 2) //if directionn(direction) =
2(backward)
{
back(8); // backward(car)
turnL(2); //slightly move leftwards(prevent from being stuck in blind alley)
Serial.print(“ Reverse “); //display direction(backward)
}
if(directionn == 6) //if directionn(direction) = 6(rightward)
{ back(1);
turnR(6); // turn right
Serial.print(“ Right “); //display direction(turn left)
}
if(directionn == 4) //if directionn(direction) = 4(turn left)
{ back(1);
turnL(6); // turn left
Serial.print(“ Left “); //display direction(turn right)
}
if(directionn == 8) //if directionn(direction) = 8(forward)
{
advance(1); // go forward as normal
Serial.print(“ Advance “); //display direction(forward)
Serial.print(“ “);
}
}

22

You might also like