Skip to content

Instantly share code, notes, and snippets.

@chepecarlos
Created March 23, 2021 20:55
Show Gist options
  • Save chepecarlos/d2ca3e182aa91d16af469d33f5a1cf63 to your computer and use it in GitHub Desktop.
Save chepecarlos/d2ca3e182aa91d16af469d33f5a1cf63 to your computer and use it in GitHub Desktop.
Codigo con inicio raro
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