Created
March 23, 2021 20:55
-
-
Save chepecarlos/d2ca3e182aa91d16af469d33f5a1cf63 to your computer and use it in GitHub Desktop.
Codigo con inicio raro
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
include <AFMotor.h> //libreria adafrit motor(shield) | |
#include <NewPing.h> //librería para sensor ultrasónico | |
#include <Servo.h> //libreria para servomotores | |
#define TRIG_PIN A0 //TRIG en pin A0 | |
#define ECHO_PIN A1 //ECO en pin A1 | |
#define MAX_DISTANCE 200 //distancia maxima de lectura | |
#define MAX_SPEED 190 //potencia mexima del motor | |
#define MAX_SPEED_OFFSET 20 | |
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); | |
AF_DCMotor motor1(1, MOTOR12_1KHZ); //motor conectado al conector 1 a frecuencia de 1 KHZ | |
AF_DCMotor motor2(2, MOTOR12_1KHZ); //motor conectado al conector 2 a frecuencia de 1 KHZ | |
AF_DCMotor motor3(3, MOTOR34_1KHZ); //motor conectado al conector 3 a frecuencia de 1 KHZ | |
AF_DCMotor motor4(4, MOTOR34_1KHZ); //motor conectado al conector 4 a frecuencia de 1 KHZ | |
Servo myservo; | |
boolean goesForward = false; //variable booleana que hace que el carro no camime delante | |
int speedSet = 0; //variable que almacena la velocidad del bucle for | |
void setup() { | |
myservo.attach(10); //pin del servo al que se conecta el servo | |
myservo.write(115); //se escribe un ángulo de 115 grados | |
delay(2000); //espera de dos segundos | |
} | |
void loop() { | |
int distance = readPing(); //leer distancia | |
int distanceR = 0; //variable distancia a la derecha | |
int distanceL = 0; //variable distancia a la izquierda | |
delay(40); //espera 40 milisegundos | |
if (distance <= 15) { //condicional | |
moveStop(); //alto | |
delay(100); | |
moveBackward(); //reversa | |
delay(300); | |
moveStop(); //alto | |
delay(200); | |
distanceR = lookRight(); //leer distancia derecha | |
delay(200); | |
distanceL = lookLeft(); //leer distancia izquierda | |
delay(200); | |
if (distanceR >= distanceL) { //condicional | |
turnRight(); //ir a la derecha | |
moveStop(); //alto | |
} else { | |
turnLeft(); //ir a la izquierda | |
moveStop(); //alto | |
} | |
} else { | |
moveForward(); //ir adelante | |
} | |
} | |
//metodos: | |
int lookRight() { | |
myservo.write(50); //servo a 50 grados | |
delay(500); | |
int distance = readPing(); //leer distancia | |
delay(100); | |
myservo.write(115); //regreso a reposo 115 grados | |
return distance; //regresar distancia | |
} | |
int lookLeft() { | |
myservo.write(170); //servo a 170 grados | |
delay(500); | |
int distance = readPing(); //leer distancia | |
delay(100); | |
myservo.write(115); //regreso a 115 grados | |
return distance; //regresar distancia | |
delay(100); | |
} | |
int readPing() { //función readPing | |
delay(70); | |
int cm = sonar.ping_cm(); //lanzar señal para medir en cm | |
if (cm == 0) //condicional | |
{ | |
cm = 250; // cm=250 | |
} | |
return cm; //regresar cm | |
} | |
void moveStop() { //función alto | |
motor1.run(RELEASE); //motor 1 detenido | |
motor2.run(RELEASE); //motor 2 detenido | |
motor3.run(RELEASE); //motor 3 detenido | |
motor4.run(RELEASE); //motor 4 detenido | |
} | |
void moveForward() { //funcion adelante | |
if (!goesForward) //condicional | |
{ | |
goesForward = true; //ir adelanté truee | |
motor1.run(FORWARD); //motor 1 adelante | |
motor2.run(FORWARD); //motor 2 adelante | |
motor3.run(FORWARD); //motor 3 adelante | |
motor4.run(FORWARD); //motor 4 adelante | |
for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) | |
{ | |
motor1.setSpeed(speedSet); //speedset aumenta y se coloca de velocidad al motor 1 | |
motor2.setSpeed(speedSet); //speedset aumenta y se coloca de velocidad al motor 2 | |
motor3.setSpeed(speedSet); //speedset aumenta y se coloca de velocidad al motor 3 | |
motor4.setSpeed(speedSet) //speedset aumenta y se coloca de velocidad al motor 4 | |
delay(5); | |
} | |
} | |
} | |
void moveBackward() { //función reversa | |
goesForward = false; //ir adelante falso | |
motor1.run(BACKWARD); //motor 1 reversa | |
motor2.run(BACKWARD); //motor 2 reversa | |
motor3.run(BACKWARD); //motor 3 reversa | |
motor4.run(BACKWARD); //motor 4 reversa | |
for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) { | |
motor1.setSpeed(speedSet); //speedset aumenta y se coloca de velocidad al motor 1 | |
motor2.setSpeed(speedSet); //speedset aumenta y se coloca de velocidad al motor 2 | |
motor3.setSpeed(speedSet); //speedset aumenta y se coloca de velocidad al motor 3 | |
motor4.setSpeed(speedSet); //speedset aumenta y se coloca de velocidad al motor 4 | |
delay(5); | |
} | |
} | |
void turnRight() { //función ir a la derecha | |
motor1.run(FORWARD); //motor 1 adelante | |
motor2.run(FORWARD); //motor 2 adelante | |
motor3.run(BACKWARD); //motor 3 reversa | |
motor4.run(BACKWARD); //motor 4 reversa | |
delay(500); | |
motor1.run(FORWARD); //motor 1 adelante | |
motor2.run(FORWARD); //motor 2 adelante | |
motor3.run(FORWARD); //motor 3 adelante | |
motor4.run(FORWARD); //motor 4 adelante | |
} | |
void turnLeft() { //función ir a la izquierda | |
motor1.run(BACKWARD); //motor 1 revesa | |
motor2.run(BACKWARD); //motor 2 reversa | |
motor3.run(FORWARD); //motor 3 adelante | |
motor4.run(FORWARD); //motor 4 adelante | |
delay(500); | |
motor1.run(FORWARD); //motor 1 reversa | |
motor2.run(FORWARD); //motor 1 reversa | |
motor3.run(FORWARD); //motor 1 reversa | |
motor4.run(FORWARD); //motor 1 reversa | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment