inicial

MinisumoTapatioMotorBoard.ino

@@ -0,0 +1,165 @@
+/*
+Codigo base para robot RC, utilizando la placa Tapatio Motor Board
+By: Miguel Angel Delgado López - miguel.delgado.lop@gmail.com
+*/
+
+//Velocidad con la que quieres que se mueva el robot
+//debe ser un valor máximo de 255 y dependerá de lo motores que uses
+int power=150;
+
+//Pines necesario para controlar los diver Tapatio Driver
+#define M01 6
+#define M02 5
+#define M03 10
+#define M04 9
+
+//Pines para los sensores P= Principal I= Izquierdo D= Derecho
+#define sensP A0
+#define sensI A3
+#define sensD A7
+
+
+#define led1  13
+
+int     estrategia = 0;
+boolean start       = true;
+boolean inicio      = true;
+int     umbral      = 900;
+
+int tiempoReversa=.5; //en segundos
+
+
+
+
+//Configuracion de los pines como salida e iniciar el Bluetooth
+void setup() {
+  Serial.begin(9600);
+  pinMode(M01,OUTPUT);
+  pinMode(M02,OUTPUT);
+  pinMode(M03,OUTPUT);
+  pinMode(M04,OUTPUT);
+  //-----------SENSORES----------------
+  pinMode(sensP,INPUT);
+  pinMode(sensI,INPUT);
+  pinMode(sensD,INPUT);
+
+}
+
+//Método principal que al recibir una letra hace que se mueva el robot RC
+void loop() {
+
+  if(!start){
+    velocidad(0, 0);
+    int x= leerSensores();
+    Serial.println(x);
+  }else{
+    if(inicio){
+      for(int x=0;x<30;x++){
+        led(0);
+        delay(50);
+        led(1);
+        delay(50);
+      }
+      inicio=false;
+    }else{
+      int sensores=leerSensores();
+      if(sensores==0){
+          velocidad(power,power);
+          Serial.println("adelante");
+      }else{
+        if(sensores==1){
+          switch(estrategia){
+            case 0:
+              //Frente por izquierda
+              velocidad(power,-power);
+              Serial.println("Frente Izquieda");    
+              break;
+            case 1:
+              //Frente por la derecha
+              velocidad(-power,power);
+              Serial.println("Frente Derecha");
+              break;
+            case 2:
+              //Atras por izquierda
+              velocidad(-power,-power*.3);
+              Serial.println("atras izquierda");
+              break;  
+            case 3:
+              //atras por derecha
+              velocidad(-power*.3,-power);
+              Serial.println("atras derecha");
+              break;
+            }
+          }
+        if(sensores==2|| sensores==3){
+          //case 1://se activo sensor izquierdo
+          Serial.println("izquerdo");
+          velocidad(-power,-power);
+          delay(tiempoReversa*1000);
+        }
+
+        if(sensores==4|| sensores==5){
+          //case 2://Se activo sensor derecho
+          Serial.println("derecho");
+          velocidad(-power,-power);
+          delay(tiempoReversa*1000);
+        }
+
+        if(sensores==6||sensores==7){
+          // case 3://se activaron los 2 sensores
+          //reversa
+          Serial.println("ambos");
+          velocidad(-power,-power);
+          delay(tiempoReversa*1000);
+        }
+      }         
+    }
+  }
+}
+
+
+  int leerSensores(){
+  int v=0;
+  v+=digitalRead(sensP);
+  v+=analogRead(sensI)<umbral?2:0;
+  v+=analogRead(sensD)<umbral?4:0;
+  return v;
+}
+
+void led(bool l1){
+  digitalWrite(led1,l1);
+}
+
+
+//Método que permite controlar la velocidad y sentido de giro a los motores
+//El valor minimo es -255 y el máximo 255, Negativo hacia atrás y positivo hacia adelante
+void velocidad(int izq, int der){
+  if(izq>255){
+    izq=255;
+  }else{
+    if(izq<-255){
+      izq=-255;
+    }
+  }
+  if(der>255){
+    der=255;
+  }else{
+    if(der<-255){
+      der=-255;
+    }
+  }
+  if(izq>0){
+    analogWrite(M01,izq);
+    analogWrite(M02, 0);
+  }else{
+    analogWrite(M01, 0);
+    analogWrite(M02, abs(izq));
+  }
+  if(der>0){
+    analogWrite(M03,der);
+    analogWrite(M04, 0);
+  }else{
+    analogWrite(M03, 0);
+    analogWrite(M04, abs(der));
+  }
+}