Diff: main_copy.cpp

Revision:

3:f850f53ff658

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_copy.cpp	Tue Dec 03 03:44:05 2019 +0000
@@ -0,0 +1,176 @@
+#include "mbed.h"
+#include "Servo.h"
+
+/*****************
+Código base para o controlador P. 
+
+O sistema fica no aguardo do usuário para começar.
+Manda um cabeçalho na serial.
+Depois ele pisca o led @ 5  Hz, 50 vezes.
+Ele liga os motores em IDLE.
+Manda um aviso de pronto.
+Começa o loop de controle @ 50 Hz
+    - Le ADC ( Analogic Digital COnverter)
+     - Manda o valor lido pelo ad
+*****************/
+
+#define MOTOR_RESET_CONDITION 0.0f
+#define MOTOR_IDLE_CONDITION 0.05f
+
+//Não mexer - Definição dos Pinos da Bancada    
+DigitalOut myled(LED1);
+InterruptIn button(USER_BUTTON);
+Servo motorEsquerda(D5);
+Servo motorDireita(D6);
+AnalogIn encoder(PB_0);
+Serial pc(SERIAL_TX, SERIAL_RX);
+Ticker looper;
+
+bool _btnFunction=false;
+bool loopFlag = false;
+float ref = 0.0f;
+uint16_t angle;
+
+void pressed()
+{
+   _btnFunction = !_btnFunction;
+}
+
+void wait_user()
+{
+    while(!_btnFunction) {
+        wait_ms(10);
+    }
+    _btnFunction = 0;
+}
+
+void blink_led(int period_in_ms, int times)
+{
+    for(int i=0; times; i++) {
+        myled = !myled;
+        wait_ms(period_in_ms);
+    }
+    myled=0;
+}
+
+void loop()
+{
+    loopFlag = true;
+}
+
+int main() {
+    float janela;//[4] = {0, 0, 0, 0};
+    float angulo_degree = 0;
+    float angulo_media;
+    float P = 0.002; //v4 3*0.001;//3*0.00001;
+    //float I = 1.6*5;//v4 1.6*50;
+    //float D = 3.5/200;//v4 3.5/50;
+    float I = 0.0002;//v4 1.6*50;
+    float D = 0.0005;//v4 3.5/50;
+    float CP = 0;
+    float CI = 0;
+    float CD = 0;    
+    float e_atual = 0;
+    float e_anterior = 0;
+    float integral_atual = 0;
+    float integral_anterior = 0;
+    float integral_temp = 0;
+    float Ts = 0.02;
+    float derivada = 0;
+    float angulo_V;
+    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);
+     motorDireita.write(MOTOR_RESET_CONDITION);     
+     myled = 1;
+     wait_user();
+     myled = 0;
+     pc.printf("Nucleo F401 turned on!\r\n");
+     //blink_led(20, 50);
+     motorDireita.write(MOTOR_IDLE_CONDITION);
+     motorEsquerda.write(MOTOR_IDLE_CONDITION);
+     pc.printf("Control should start in 10 s.\r\n");
+     wait(1);
+     looper.attach(&loop,0.02); 
+     float sumError=0;   
+     while(1) 
+     {
+         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){
+                angle =  encoder.read_u16()>>4; //Tensão em 12 bits
+                /******
+                Espaço reservado para colocar a ação de controle
+                O exemplo é de um P
+                ************/
+                angulo_V = 0.000806*angle;
+                angulo_degree = 27.779*angulo_V - 41.1;
+                janela = janela + angulo_degree;
+                k++;
+                } 
+
+            angulo_media = janela/k; //(janela[0]+janela[1]+janela[2]+janela[3])/4;
+            e_anterior = e_atual;
+            e_atual = -(angulo_media - ref);
+            CP = P * e_atual;
+            
+            
+            integral_atual = integral_anterior + (e_atual + e_anterior)*Ts/2;
+            integral_anterior = integral_atual;
+            
+            //CI = P*I*integral_atual;
+            CI = I*integral_atual;
+            
+            derivada = (e_atual - e_anterior)/Ts;
+            //CD = P*D*derivada;
+            CD = D*derivada;
+            
+            controlAction = CP+CI+CD;
+            if (controlAction >= 0.45){
+                controlAction  = 0.45;
+                }
+           
+            if (controlAction <= -0.45){
+                controlAction  = -0.45;
+            }
+            
+            
+            motorEsquerda.write((0.8*(0.5 - controlAction)));
+            motorDireita.write(0.5 + 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;
+            }
+         }
+          
+      }
+}
\ No newline at end of file