398 lines
8.4 KiB
C++
398 lines
8.4 KiB
C++
#include <Servo.h>
|
|
|
|
#define debug 1
|
|
|
|
uint16_t val_shift; //Valor del shift register
|
|
const uint8_t dataPin=5;
|
|
const uint8_t latchPin=6;
|
|
const uint8_t clockPin=7;
|
|
|
|
Servo servo0; //giro
|
|
Servo servo1; //elevacion pinza
|
|
Servo servo2; //pinza1
|
|
Servo servo3; //pinza2
|
|
|
|
byte pos_servo0 = 90;
|
|
|
|
#define sel 9
|
|
|
|
#define m0a 10 // TRACCION
|
|
#define m0b 11
|
|
#define m1a 12 // ELEVADOR
|
|
#define m1b 13
|
|
#define m2a 14 // GATO
|
|
#define m2b 15
|
|
#define m3a 4 // CINTA
|
|
#define m3b 5
|
|
#define m4a 6 // DIRECCION
|
|
#define m4b 7
|
|
|
|
#define led0 1 // LAMP0 - OJOS
|
|
#define led1 2 // LAMP1 - CUERPO
|
|
#define led2 3 // LAMP2 - TRASERO
|
|
|
|
#define fc0 19 // AN5 - ABAJO ELEVADOR
|
|
#define fc1 18 // AN4 - ARRIBA ELEVADOR
|
|
#define fc2 17 // AN3 - GATO ELEVADOR
|
|
#define fc3 16 // AN2 - INTERMEDIO ELEVADOR
|
|
#define fc4 13 // CENTRO DIRECCION
|
|
#define fc5 12 //
|
|
|
|
#define sensor0 1 //AN1
|
|
#define sensor1 0 //AN0
|
|
|
|
#define time_exp 25000 //Tiempo expulsion bandeja
|
|
#define time_cin 10000 //Tiempo cinta
|
|
|
|
|
|
char val_ant=0;
|
|
|
|
boolean flag_p = false;
|
|
boolean flag_der = false;
|
|
boolean flag_iz = false;
|
|
boolean flag_red = false;
|
|
boolean flag_blue = false;
|
|
boolean flag_green = false;
|
|
|
|
void resetshift()
|
|
{
|
|
val_shift = 0x0000;
|
|
digitalWrite(latchPin, LOW);
|
|
byte registerTwo = highByte(val_shift);
|
|
byte registerOne = lowByte(val_shift);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, registerOne);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, registerTwo);
|
|
digitalWrite(latchPin, HIGH);
|
|
}
|
|
|
|
void shiftWrite(uint8_t pin, uint8_t state)
|
|
{
|
|
bitWrite(val_shift, pin, state);
|
|
digitalWrite(latchPin, LOW);
|
|
byte registerTwo = highByte(val_shift);
|
|
byte registerOne = lowByte(val_shift);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, registerOne);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, registerTwo);
|
|
digitalWrite(latchPin, HIGH);
|
|
}
|
|
|
|
int average(int anaPin)
|
|
{
|
|
int lecturas=4;
|
|
long total = 0;
|
|
long average = 0;
|
|
int count = 0;
|
|
for(int i=0; i<lecturas; i++)
|
|
{
|
|
total = total + analogRead(anaPin);
|
|
}
|
|
|
|
average = total / lecturas;
|
|
return(average);
|
|
|
|
}
|
|
|
|
void ini()
|
|
{
|
|
|
|
servo0.write(90);
|
|
delay(15);
|
|
if(debug)
|
|
Serial.println("servo0 -> 90");
|
|
|
|
servo1.write(120); //Pinza vertical 120(Arriba) 20(abajo)
|
|
delay(15);
|
|
if(debug)
|
|
Serial.println("servo1 -> 120");
|
|
|
|
for(int i=0; i<1; i++)
|
|
{
|
|
cerrar();
|
|
if(debug)
|
|
Serial.println("cerrar");
|
|
delay(1000);
|
|
abrir();
|
|
if(debug)
|
|
Serial.println("abrir");
|
|
delay(1000);
|
|
}
|
|
if(debug)
|
|
Serial.println("recogiendo...");
|
|
recoge();
|
|
if(debug)
|
|
Serial.println("bajando...");
|
|
go_red();
|
|
}
|
|
|
|
|
|
|
|
void setup()
|
|
{
|
|
|
|
Serial.begin(57600);
|
|
servo0.attach(8);
|
|
servo1.attach(9);
|
|
servo2.attach(10);
|
|
servo3.attach(11);
|
|
|
|
pinMode(dataPin, OUTPUT);
|
|
pinMode(latchPin, OUTPUT);
|
|
pinMode(clockPin, OUTPUT);
|
|
pinMode(fc0, INPUT); //Inferior
|
|
pinMode(fc1, INPUT); //Central
|
|
pinMode(fc2, INPUT); //Superior
|
|
pinMode(fc3, INPUT); //Final de carrera de expulsion
|
|
pinMode(fc4, INPUT);
|
|
pinMode(fc5, INPUT);
|
|
digitalWrite(fc0, HIGH);
|
|
digitalWrite(fc1, HIGH);
|
|
digitalWrite(fc2, HIGH);
|
|
digitalWrite(fc3, HIGH);
|
|
digitalWrite(fc4, HIGH);
|
|
digitalWrite(fc5, HIGH);
|
|
resetshift();
|
|
shiftWrite(sel, HIGH);
|
|
ini();
|
|
}
|
|
|
|
void loop() {
|
|
|
|
if (Serial.available())
|
|
{
|
|
char valor = Serial.read();
|
|
if (valor!=val_ant)
|
|
{
|
|
if (valor == 'w') avanza();
|
|
else if (valor == 's') retrocede();
|
|
else if ((valor == 'a')&&(!flag_iz)) { izquierda(); flag_iz = true;}
|
|
else if ((valor == 'd')&&(!flag_der)) { derecha(); flag_der = true;}
|
|
else if (valor == 'p') { stop_motor(); flag_der = false; flag_iz = false;}
|
|
//else if (valor == 'i') abrir();
|
|
//else if (valor == 'j') cerrar();
|
|
else if ((valor == 'r')&&(!flag_red))
|
|
{
|
|
flag_red = true;
|
|
go_red();
|
|
search();
|
|
expulsa_semillas(time_exp);
|
|
cerrar();
|
|
servo1.write(20); //Pinza vertical 120(Arriba) 20(abajo)
|
|
delay(15);
|
|
go_red();
|
|
cinta_av(time_cin);
|
|
abrir();
|
|
servo1.write(120); //Pinza vertical 120(Arriba) 20(abajo)
|
|
delay(15);
|
|
}
|
|
else if (valor == 't') flag_red = false;
|
|
else if ((valor == 'g')&&(!flag_green))
|
|
{
|
|
flag_green = true;
|
|
go_green();
|
|
search();
|
|
expulsa_semillas(time_exp);
|
|
cerrar();
|
|
servo1.write(20); //Pinza vertical 120(Arriba) 20(abajo)
|
|
delay(15);
|
|
go_red();
|
|
cinta_av(time_cin);
|
|
abrir();
|
|
servo1.write(120); //Pinza vertical 120(Arriba) 20(abajo)
|
|
delay(15);
|
|
}
|
|
else if (valor == 'h') flag_green = false;
|
|
else if ((valor == 'b')&&(!flag_blue))
|
|
{
|
|
flag_blue = true;
|
|
go_blue();
|
|
search();
|
|
expulsa_semillas(time_exp);
|
|
cerrar();
|
|
servo1.write(20); //Pinza vertical 120(Arriba) 20(abajo)
|
|
delay(15);
|
|
go_red();
|
|
cinta_av(time_cin);
|
|
abrir();
|
|
servo1.write(120); //Pinza vertical 120(Arriba) 20(abajo)
|
|
delay(15);
|
|
}
|
|
else if (valor == 'c') flag_blue = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void stop_motor()
|
|
{
|
|
motor0(0,0); //Motor de avance o retroceso
|
|
servo0.write(90); // sets the servo position according to the scaled value
|
|
delay(15);
|
|
}
|
|
|
|
void search()
|
|
{
|
|
while (average(0)>400) sube();
|
|
motor1(0,0);
|
|
baja();
|
|
delay(2000);
|
|
motor1(0,0);
|
|
}
|
|
|
|
void go_red()
|
|
{
|
|
|
|
while (digitalRead(fc0)) // BAJA HASTA QUE SE ACTIVE EL SENSOR INFERIOR DEL ELEVADOR
|
|
baja();
|
|
|
|
motor1(0,0);
|
|
|
|
}
|
|
|
|
void go_green()
|
|
{
|
|
while ((digitalRead(fc1))&&digitalRead(fc2)) sube(); // SUBE HASTA QUE SE ACTIVE UNA VEZ EL SENSOR INTERMEDIO O EL SUPERIOR DEL ELEVADOR
|
|
motor1(0,0);
|
|
while ((digitalRead(fc1))&&digitalRead(fc0)) baja(); // EN CASO DE LLEGAR ARRIBA, BAJA
|
|
motor1(0,0);
|
|
}
|
|
|
|
|
|
|
|
void go_blue()
|
|
{
|
|
while ((digitalRead(fc1))&&digitalRead(fc2)) sube(); // ESPERA A QUE SE ACTIVE UNA VEZ EL SENSOR INTERMEDIO O EL SUPERIOR DEL ELEVADOR
|
|
delay(1000);
|
|
while ((digitalRead(fc1))&&digitalRead(fc2)) sube(); // SUBE HASTA QUE SE ACTIVE POR SEGUNDA VEZ EL SENSOR INTERMEDIO O EL SUPERIOR DEL ELEVADOR
|
|
motor1(0,0);
|
|
// while (digitalRead(fc2)) sube(); // SUBE HASTA QUE SE ACTIVE EL SENSOR SUPERIOR DEL ELEVADOR
|
|
// motor1(0,0);
|
|
}
|
|
|
|
|
|
void sube ()
|
|
{
|
|
motor1(1,0);
|
|
}
|
|
|
|
void baja ()
|
|
{
|
|
motor1(0,1);
|
|
}
|
|
|
|
|
|
void empuja ()
|
|
{
|
|
motor2(1,0);
|
|
}
|
|
|
|
void recoge ()
|
|
{
|
|
motor2(0,1);
|
|
unsigned long time_pul = millis();
|
|
while ((millis()-time_pul)<100) { if (digitalRead(fc3)) time_pul=millis();}
|
|
motor2(0,0);
|
|
}
|
|
|
|
void expulsa_semillas(unsigned long time)
|
|
{
|
|
empuja();
|
|
delay(time);
|
|
motor2(0,0);
|
|
delay(100);
|
|
recoge();
|
|
//while (digitalRead(fc3)) recoge(); motor2(0,0);
|
|
}
|
|
void cinta_av (unsigned long time)
|
|
{
|
|
motor3(1,0);
|
|
delay(time);
|
|
motor3(0,0);
|
|
}
|
|
|
|
void cinta_re ()
|
|
{
|
|
motor3(0,1);
|
|
}
|
|
|
|
void retrocede ()
|
|
{
|
|
motor0(1,0);
|
|
}
|
|
|
|
void avanza ()
|
|
{
|
|
motor0(0,1);
|
|
}
|
|
|
|
void derecha_serv()
|
|
{
|
|
if (pos_servo0 > 180) pos_servo0 = 180;
|
|
else pos_servo0 = pos_servo0 + 10;
|
|
servo0.write(pos_servo0); // sets the servo position according to the scaled value
|
|
delay(15);
|
|
}
|
|
|
|
void izquierda_serv()
|
|
{
|
|
if (pos_servo0 < 10) pos_servo0 = 0;
|
|
else pos_servo0 = pos_servo0 - 10;
|
|
servo0.write(pos_servo0); // sets the servo position according to the scaled value
|
|
delay(15);
|
|
}
|
|
|
|
void derecha()
|
|
{
|
|
motor4(1,0);
|
|
delay(100);
|
|
motor4(0,0);
|
|
}
|
|
|
|
void izquierda()
|
|
{
|
|
motor4(0,1);
|
|
delay(100);
|
|
motor4(0,0);
|
|
}
|
|
|
|
void abrir()
|
|
{
|
|
servo3.write(80); //Pinza derecha 150(cerrada) 80(Abierta)
|
|
delay(15);
|
|
servo2.write(100); //Pinza izquierda 30(cerrada) 100(Abierta)
|
|
delay(15);
|
|
}
|
|
|
|
void cerrar()
|
|
{
|
|
servo3.write(150); //Pinza derecha 150(cerrada) 80(Abierta)
|
|
delay(15);
|
|
servo2.write(30); //Pinza izquierda 30(cerrada) 100(Abierta)
|
|
delay(15);
|
|
}
|
|
|
|
void motor0 (boolean sent, boolean sent1) { //Motor de avance
|
|
shiftWrite(m0a, sent);
|
|
shiftWrite(m0b, sent1);
|
|
}
|
|
|
|
void motor1 (boolean sent, boolean sent1) { //Motor de torre
|
|
shiftWrite(m1a, sent);
|
|
shiftWrite(m1b, sent1);
|
|
}
|
|
|
|
void motor2 (boolean sent, boolean sent1) { //Motor expulsion
|
|
shiftWrite(m2a, sent);
|
|
shiftWrite(m2b, sent1);
|
|
}
|
|
|
|
void motor3 (boolean sent, boolean sent1) { //Motor cinta
|
|
shiftWrite(m3a, sent);
|
|
shiftWrite(m3b, sent1);
|
|
}
|
|
|
|
void motor4 (boolean sent, boolean sent1) {
|
|
shiftWrite(m4a, sent);
|
|
shiftWrite(m4b, sent1);
|
|
}
|
|
|