119 lines
1.8 KiB
Arduino
119 lines
1.8 KiB
Arduino
|
#define m1a 9
|
||
|
|
#define m1b 2
|
||
|
|
#define m2a 10
|
||
|
|
#define m2b 3
|
||
|
|
#define m3 11
|
||
|
|
#define fc1 18 //4
|
||
|
|
#define fc2 19 //5
|
||
|
|
#define f_iz 0
|
||
|
|
#define f_der 1
|
||
|
|
#define f_cen 2
|
||
|
|
#define t_cen 3
|
||
|
|
|
||
|
|
boolean flag_der = false;
|
||
|
|
boolean flag_izq = false;
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
|
||
|
|
Serial.begin(57600);
|
||
|
|
pinMode(m1a, OUTPUT);
|
||
|
|
pinMode(m2a, OUTPUT);
|
||
|
|
pinMode(m1b, OUTPUT);
|
||
|
|
pinMode(m2b, OUTPUT);
|
||
|
|
pinMode(m3, OUTPUT);
|
||
|
|
pinMode(fc1, INPUT);
|
||
|
|
pinMode(fc2, INPUT);
|
||
|
|
digitalWrite(m1a, LOW);
|
||
|
|
digitalWrite(m2a, LOW);
|
||
|
|
digitalWrite(fc1, HIGH);
|
||
|
|
digitalWrite(fc2, HIGH);
|
||
|
|
digitalWrite(m1b, LOW);
|
||
|
|
digitalWrite(m2b, LOW);
|
||
|
|
|
||
|
|
stop_motores();
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
|
||
|
|
if (Serial.available())
|
||
|
|
{
|
||
|
|
char valor = Serial.read();
|
||
|
|
if (valor == 'w') avanza();
|
||
|
|
else if (valor == 's') retrocede();
|
||
|
|
else if ( (valor == 'a') && (!flag_der) ) izquierda();
|
||
|
|
else if ( (valor == 'd') && (!flag_izq) ) derecha();
|
||
|
|
else if (valor == 'p') stop_motores();
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
Serial.print("fc1 ");
|
||
|
|
Serial.print(digitalRead(fc1));
|
||
|
|
Serial.print("fc2 ");
|
||
|
|
Serial.println(digitalRead(fc2));
|
||
|
|
|
||
|
|
|
||
|
|
if( !digitalRead(fc2) && !flag_der )
|
||
|
|
{
|
||
|
|
flag_der = true;
|
||
|
|
motor1(0,0);
|
||
|
|
}
|
||
|
|
else if (digitalRead(fc2))
|
||
|
|
flag_der = false;
|
||
|
|
|
||
|
|
if( !digitalRead(fc1) && !flag_izq )
|
||
|
|
{
|
||
|
|
flag_izq = true;
|
||
|
|
motor1(0,0);
|
||
|
|
}
|
||
|
|
else if (!digitalRead(fc1))
|
||
|
|
flag_izq = false;
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void cabeza(int c)
|
||
|
|
{
|
||
|
|
digitalWrite(m3, c);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void stop_motores()
|
||
|
|
{
|
||
|
|
motor2(0,0);
|
||
|
|
motor1(0,0);
|
||
|
|
cabeza(0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void retrocede ()
|
||
|
|
{
|
||
|
|
motor2(1,0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void avanza ()
|
||
|
|
{
|
||
|
|
motor2(0,1);
|
||
|
|
}
|
||
|
|
|
||
|
|
void derecha()
|
||
|
|
{
|
||
|
|
motor1(1,0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void izquierda()
|
||
|
|
{
|
||
|
|
motor1(0,1);
|
||
|
|
}
|
||
|
|
|
||
|
|
void motor1 (boolean sent, boolean sent1) {
|
||
|
|
digitalWrite(m1a, sent);
|
||
|
|
digitalWrite(m1b, sent1);
|
||
|
|
}
|
||
|
|
|
||
|
|
void motor2 (boolean sent, boolean sent1) {
|
||
|
|
digitalWrite(m2a, sent);
|
||
|
|
digitalWrite(m2b, sent1);
|
||
|
|
}
|
||
|
|
|
||
|
|
|