Created
January 7, 2016 21:37
-
-
Save Nullpo/bdc88b77f4059cb38e83 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| //18/11/2015 | |
| // KITROBOT | |
| // DAniel N. Esquerdo | |
| // Controlador robot Rover Micro RM3 version 0_1 | |
| // Sensores : Bumper derecho e izquierdo | |
| // Sensor de distancia ultrasonico modelo HC-SR04 | |
| // Servos : 2 | |
| // Servo 1: IZQUIERDO | |
| // Servo 2: DERECHO | |
| // Comportamiento : Avanza y si se acciona un bumper gira. | |
| // Pulsando "c" se calibra la posicion neutral de los servos | |
| // Pulsando "a" incrementa posicion neutral de servo Izquierdo | |
| // Pulsando "z" decrementa posicion neutral de servo Izquierdo | |
| // Pulsando "s" incrementa posicion neutral de servo Derecho | |
| // Pulsando "x" decrementa posicion neutral de servo Derecho | |
| #include <Servo.h> | |
| #include "Ultrasonic.h" | |
| int RED = 1; | |
| int BLUE = 0; | |
| Ultrasonic ultrasonic(11,12); // (Trig PIN,Echo PIN) | |
| Servo servo_der; | |
| Servo servo_izq; | |
| int neutro_der = 88; // Posicion detenido o neutral de rueda derecha | |
| int neutro_izq = 97; // Posicion detenido o neutral de rueda izquierda .032 | |
| int bumper_der = 4; //conectado a entrada digital 4 | |
| int bumper_izq = 5; //conectado a entrada digital 5 | |
| int f_bumper_der = 1; | |
| int f_bumper_izq = 1; | |
| int analogPin = 0; // el pin analógico 0 conectado al dial de un potenciómetro (terminal central del potenciómetro) | |
| long time_ultra = 2; | |
| long cont_ultra=0; | |
| int distancia_cm=0; | |
| int led = 13; | |
| int byte_entrante=0; | |
| long accion=1; | |
| int f_calibrar=0; | |
| int LED_ROJO = 6; | |
| int LED_AZUL = 8; | |
| int SONIDO = 0; //VER | |
| void setup() | |
| { | |
| pinMode(LED_ROJO, OUTPUT); | |
| pinMode(LED_AZUL, OUTPUT); | |
| /*pinMode(LED_ROJO, OUTPUT); | |
| pinMode(LED_AZUL, OUTPUT); | |
| digitalWrite(LED_AZUL, HIGH); | |
| digitalWrite(LED_ROJO, HIGH);*/ | |
| pinMode(bumper_der, INPUT); | |
| digitalWrite(bumper_der, HIGH); // activa la resistencia pullup | |
| pinMode(bumper_izq, INPUT); | |
| digitalWrite(bumper_izq, HIGH); // activa la resistencia pullup | |
| pinMode(led, OUTPUT); | |
| Serial.begin(115200); | |
| servo_der.attach(3); | |
| servo_izq.attach(2); | |
| servo_der.write(neutro_der); | |
| servo_izq.write(neutro_izq); | |
| delay(500); | |
| } | |
| unsigned long timeHuida = 0; | |
| unsigned long timeLed = 0; | |
| int ledDefaultThreshold = 1000; | |
| int ledScaredThreshold = 100; | |
| int ledThreshold = ledDefaultThreshold; | |
| int ledStatus = RED; | |
| void alterarLed(){ | |
| if(millis() - timeLed > ledThreshold){ | |
| if(ledStatus == RED){ | |
| digitalWrite(LED_ROJO, LOW); | |
| digitalWrite(LED_AZUL, HIGH); | |
| ledStatus = BLUE; | |
| } else { | |
| digitalWrite(LED_ROJO, HIGH); | |
| digitalWrite(LED_AZUL, LOW); | |
| ledStatus = RED; | |
| } | |
| timeLed = millis(); | |
| } | |
| } | |
| void loop(){ | |
| // Siempre altero los leds. | |
| alterarLed(); | |
| //--------------------------------------------------------------------------------------------------------------------------------- | |
| //-- CALIBRACION DE SERVOS PARA OBTENER LA POSICION NEUTRAL DE LOS MISMOS | |
| //-- PARA ACCEDER A MODO CALIBRACION ABRIR MONITOR SERIAL Y PULSAR "c" MINUSCULA | |
| if (Serial.available() > 0) { //Pulsando "c" calibro neutros | |
| byte_entrante = Serial.read(); | |
| if (byte_entrante==99) { | |
| f_calibrar=1; | |
| servo_der.write(neutro_der); //detenido | |
| servo_izq.write(neutro_izq); //detenido | |
| } | |
| } | |
| if (f_calibrar==1){ | |
| calibrar: | |
| if (Serial.available() > 0) { | |
| byte_entrante = Serial.read(); | |
| } | |
| Serial.print("\t""IZQ="); | |
| Serial.print(neutro_izq); | |
| Serial.print("\t""DER="); | |
| Serial.print(neutro_der); | |
| Serial.println("\t"" e = EXIT"); | |
| if (byte_entrante==97) {++neutro_izq;delay(1);byte_entrante=0;} // Pulsando "a" incremento neutro izquierdo | |
| if (byte_entrante==122) {--neutro_izq;delay(1);byte_entrante=0;}// Pulsando "z" decremento neutro izquierdo | |
| if (byte_entrante==115) {++neutro_der;delay(1);byte_entrante=0;}// Pulsando "s" incremento neutro derecho | |
| if (byte_entrante==120) {--neutro_der;delay(1);byte_entrante=0;}// Pulsando "x" decremento neutro derecho | |
| servo_der.write(neutro_der); //avanzar | |
| servo_izq.write(neutro_izq); //avanzar | |
| goto calibrar; | |
| } | |
| //--------------------------------------------------------------------------------------------------------------------------------- | |
| sensores(); | |
| if (accion == 1){ | |
| ledThreshold = ledDefaultThreshold; | |
| if(distancia_cm >= 20){ | |
| accion = girateUnPocoYBancala(25,-5,200); // PATRULLA | |
| } else if(distancia_cm > 15){ | |
| accion = random(2, 4); //SIEMPRE HUYE!!!! | |
| } else { | |
| accion = 4; | |
| } | |
| } else if(accion == 2) { | |
| accion = girateUnPocoYBancala(80,30, random(350, 450)); | |
| } else if(accion == 3){ | |
| accion = girateUnPocoYBancala(-80, -30, random(350, 450)); | |
| } else if(accion == 4) { | |
| timeHuida = millis(); | |
| ledThreshold = ledScaredThreshold; | |
| girateUnPocoYBancala(180,110/2, 600); // gira | |
| gritaWacho(); | |
| accion = 5; | |
| } else if(accion == 5){ | |
| if(distancia_cm < 20) { | |
| accion = 1; | |
| } else if(millis() - timeHuida < 3000){ | |
| girateUnPocoYBancala(100,-150,0); // HUYE | |
| } else { | |
| accion = 1; | |
| } | |
| } | |
| } | |
| int gritaWacho(){ | |
| } | |
| int girateUnPocoYBancala(int desviacionDer, int desviacionIzq, int delayBobi){ | |
| servo_der.write(neutro_der + desviacionDer); | |
| servo_izq.write(neutro_izq + desviacionIzq); | |
| delay(delayBobi); | |
| return 1; | |
| } | |
| int sensores(){ | |
| f_bumper_der = digitalRead(bumper_der); // leer el pin de bumper derecho y actualizar flag | |
| f_bumper_izq = digitalRead(bumper_izq); // leer el pin de bumper izquierdo y actualizar flag | |
| ++cont_ultra; | |
| if (cont_ultra>=time_ultra){ | |
| distancia_cm = ultrasonic.Ranging(CM); // CM or INC | |
| cont_ultra=0; | |
| Serial.print(distancia_cm); | |
| Serial.print(" "); | |
| Serial.print("BANANA"); | |
| Serial.println(accion); | |
| } | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment