-
Notifications
You must be signed in to change notification settings - Fork 0
/
Demax
128 lines (118 loc) · 3.19 KB
/
Demax
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
/*
Prueba Dos Motores V1
by Lucila Mortara
*/
// Conectar los pines del controlador de motores a los pines digitales del Arduino
// Motor 1 -> Motores Lado Izquierdo
int enA = 10;
int in1 = 9;
int in2 = 8;
// Motor 2 -> Motores Lado Derecho
int enB = 5;
int in3 = 7;
int in4 = 6;
// Constantes para definir para donde se mueve
#define MOTOR_DIR_ADELANTE 0
#define MOTOR_DIR_ATRAS 1
// Contador Demo
int demo = 0;
// Botonito
//int pinboton = 2;
//int estadoboton = 0;
// Sensor Ultrasonido
#include <NewPing.h>
#define TRIGGER_PIN 2 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 3 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 100 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
void setup()
{
// Setea todos los pins de control como salidas
pinMode(enA, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
//pinMode(pinboton, INPUT);// Boton
Serial.begin(9600); // Open serial monitor at 115200 baud to see ping results.
delay(5000);
}
void loop()
{
//estadoboton = digitalRead(pinboton);
if(demo<5 ){
delay(50); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
Serial.print("Ping: ");
Serial.print(uS / US_ROUNDTRIP_CM); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
Serial.println("cm");
if (uS / US_ROUNDTRIP_CM < 10 ){
// Muy cerca de un objeto
// Dobla a la izquierda
Serial.println("Muy cerca. Doblo a la izquierda");
motorDireccion(1, MOTOR_DIR_ADELANTE);
seteaVelocidad(1, 200);
motorDireccion(2, MOTOR_DIR_ATRAS);
seteaVelocidad(2, 200);
} else {
// No tan cerca
// Prende motor 1 y 2 -> Adelante" y Pone la velocidad de ambos motores a 200
Serial.println("No tan cerca. Sigo derecho");
motorDireccion(1, MOTOR_DIR_ADELANTE);
seteaVelocidad(1, 200);
motorDireccion(2, MOTOR_DIR_ADELANTE);
seteaVelocidad(2, 200);
}
delay(1000);
demo++;
}
// Apaga motores
motorPara(1);
motorPara(2);
}
void motorDireccion(char motor_num, byte direccion)
{
if (motor_num == 1) {
if (direccion == MOTOR_DIR_ADELANTE) {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
} else {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
}
} else {
if (direccion == MOTOR_DIR_ADELANTE) {
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
} else {
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
}
}
void seteaVelocidad(char motor_num, char motor_speed)
{
if (motor_num == 1)
{
analogWrite(enA, motor_speed);
}
else
{
analogWrite(enB, motor_speed);
}
}
void motorPara(char motor_num)
{
seteaVelocidad(motor_num, 0);
if (motor_num == 1)
{
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
}
else
{
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
}