Diff: main.cpp

Revision:

3:f850f53ff658

Parent:

2:0d6718386397

Child:

4:7d74fc55c844

--- a/main.cpp	Tue Dec 03 02:05:28 2019 +0000
+++ b/main.cpp	Tue Dec 03 03:44:05 2019 +0000
@@ -81,6 +81,12 @@
     float angulo_d;
     float controlAction = 0;
         
+    //Estou setando os dois setpoints aqui, serao os limiares que indicarao quando a referencia sera trocara e o movimento invertido
+    float setpoint1 = 10;
+    float setpoint2 = -setpoint1;
+    bool subida = true;
+        
+        
      pc.baud(115200);
      button.fall(&pressed);
      motorEsquerda.write(MOTOR_RESET_CONDITION);
@@ -101,6 +107,16 @@
          if(loopFlag)
          {
             loopFlag = 0;
+            
+            //DEFINICAO DO SETPOINT - subida ou descida
+            if (subida){
+                ref = setpoint1;
+                }
+            else {
+                ref = setpoint2;
+            }
+            
+            
             int k = 0;
             janela = 0;
             while (k < 4){
@@ -140,19 +156,20 @@
                 controlAction  = -0.45;
             }
             
-            //controlAction = 0.1*(ref - (float)angle);
-            
-            /*
-            if(controlAction<0.95f) {
-                controlAction= 0.95f;
-            }
-            if(controlAction>0.0f) {
-                controlAction= 0.0f;
-            }*/
             
             motorEsquerda.write((0.8*(0.5 - controlAction)));
             motorDireita.write(0.5 + controlAction);
-            pc.printf("A[%4d]U[%.2f]\r\n",angle,controlAction);    
+            pc.printf("A[%4d]U[%.2f]\r\n",angle_media,controlAction);    
+            
+            
+            //Se ele estiver subindo, mas subiu mais que o setpoint1, entao deve descer
+            //Se estiver descendo, mas desceu mais que o setpoint2, entao deve subir
+            if (subida && angulo_media>setpoint1){
+                subida = false;
+            }
+            if (!(subida) && angulo_media<setpoint2){
+                subida = true;
+            }
          }
           
       }