#include #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 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); }