Diff: main.cpp

Revision:

1:8aece97d3c78

Parent:

0:9d16fef10591

--- a/main.cpp	Sun Aug 11 00:57:52 2019 +0000
+++ b/main.cpp	Mon Aug 12 12:40:14 2019 +0000
@@ -8,23 +8,24 @@
 DigitalIn Sensor5 (p9); //Sensor digital localizado na direita do robo
 
 
-#define MIN 1060 //velocidade maxima para tras
+#define MIN 1260//1060 //velocidade maxima para tras
 #define MID ((MAX+MIN)/2) //valor para parar
 #define BUS 12 //velocidade de busca
-#define MAX 1860 //velocidade maxima para frente
+#define MAX 1660//1860 //velocidade maxima para frente
 #define r 2
 #define e 3
 #define d 4
 #define p 9
 #define b 5
+#define PARAMETER 0.600
 
 
 int i=MID;
 int k=MID;
 int total; //{right, right_diagonal, front, left_diagonal, left} valor atual da leitura
-int measure[5]={0,0,0,0,0}; // vetor que grava a leitura atual
-int tot=0;
-
+int measure[5]; // vetor que grava a leitura atual
+int tot;
+int in=20;
  
 //*****************************  DEFINICOES - FINAL  *****************************
 //################################################################################
@@ -45,16 +46,20 @@
     switch(controle)
     {
         case 1:
-            Left.pulsewidth_us(minimo); 
+            Left.pulsewidth_us(minimo);
+            wait(0.02); 
             Right.pulsewidth_us(minimo);
+            wait(0.02);
             break;
         case 2:
-            for (;k<=maximo || i<=maximo;k+=50,i+=50) 
+            //Right.period_ms(16);   
+            //Left.period_ms(16);
+            for (;k<=maximo && i<=maximo;k+=in,i+=in) 
             {
                 Left.pulsewidth_us(k); 
-                wait(0.05);
+                wait(0.02);
                 Right.pulsewidth_us(i); 
-                wait(0.05);
+                wait(0.02);
             if (k>MAX)
             {
                 k=MAX;
@@ -74,12 +79,14 @@
             }
             break;
         case 3:
-            for (;k>=minimo || i<=maximo;k-=50,i+=50) 
+            //Right.period_ms(16);   
+            //Left.period_ms(16);
+            for (;k>=minimo || i<=maximo;k-=in,i+=in) 
             {
                 Left.pulsewidth_us(k); 
-                wait(0.05);
+                wait(0.02);
                 Right.pulsewidth_us(i); 
-                wait(0.05);
+                wait(0.02);
             if (k>MAX)
             {
                 k=MAX;
@@ -100,12 +107,14 @@
             
             break;
         case 4:
-            for (;k<=maximo || i>=minimo;k+=50,i-=50) 
+            //Right.period_ms(16);   
+            //Left.period_ms(16);
+            for (;k<=maximo || i>=minimo;k+=in,i-=in) 
             {
                 Left.pulsewidth_us(k); 
-                wait(0.05);
+                wait(0.02);
                 Right.pulsewidth_us(i); 
-                wait(0.05);
+                wait(0.02);
             if (k>MAX)
             {
                 k=MAX;
@@ -126,12 +135,14 @@
             
             break;
         case 5:
-            for (;k>=minimo || i>=minimo;k-=50,i-=50) 
+            //Right.period_ms(16);   
+            //Left.period_ms(16);
+            for (;k>=minimo || i>=minimo;k-=in,i-=in) 
             {
                 Left.pulsewidth_us(k); 
-                wait(0.05);
+                wait(0.02);
                 Right.pulsewidth_us(i); 
-                wait(0.05);
+                wait(0.02);
             if (k>MAX)
             {
                 k=MAX;
@@ -158,6 +169,14 @@
     }
 }
 
+void Aceleration(int esq, int dir)
+{
+    Left.pulsewidth_us(esq);
+    wait(0.02); 
+    Right.pulsewidth_us(dir);
+    wait(0.02);
+}    
+
 int Search ()
 {
   measure[0] = Sensor1.read();
@@ -176,46 +195,57 @@
   {
      case 1: //vira para a direita 
         Drive(MAX,MIN,d);
+        Aceleration(MAX,MIN);
         break;
      
      case 10: //vira para a direita 
-        Drive(MAX,MIN,d);  
+        Drive(MAX,MIN,d);
+        Aceleration(MAX,MIN);  
         break;
      
      case 11: //vira para a direita
         Drive(MAX,MIN,d);
+        Aceleration(MAX,MIN);
         break;
     
      case 100: //Vai reto
         Drive(MAX,MIN,r);
+        Aceleration(MAX,MAX);
         break;
      
      case 110: //direita com leiruta do sensor da diagonal direita
         Drive(MAX,MIN,d);
+        Aceleration(MAX,MAX);
         break;
     
      case 1000: //vai para a esquerda 
         Drive(MAX,MIN,e);
+        Aceleration(MIN,MAX);
         break;
      
      case 1100: //esquerda com leitura do sensor da diagonal esquerda
         Drive(MAX,MIN,e);
+        Aceleration(MIN,MAX);
         break;
     
      case 1110: //vai reto com a leitura dos sensores diagonais e o da frente 
         Drive(MAX,MIN,r);
+        Aceleration(MAX,MAX);
         break;
     
      case 10000: //vai para a esquerda
         Drive(MAX,MIN,e);
+        Aceleration(MIN,MAX);
         break;
      
      case 11000: //vai para a esquerda 
         Drive(MAX,MIN,e);
+        Aceleration(MIN,MAX);
         break;
     
      case 11111: //leu todos os sensores ao mesmo tempo
         Drive(MAX,MIN,p);
+        Aceleration(MID,MID);
         break; 
      
    //todos os outros casos que são impossíveis
@@ -233,6 +263,6 @@
     tot=Search();
     Action(tot);
     }
-        return 0;
-        }
+    return 0;
+}
     
\ No newline at end of file