Diff: main.cpp

Revision:

0:802513e9e494

Child:

1:3d924dfe29d6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 25 23:13:48 2019 +0000
@@ -0,0 +1,144 @@
+#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 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 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;
+        
+     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;
+            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_d = 27.779*angulo_V - 41.1;
+            e_anterior = e_atual;
+            e_atual = -(angulo_d - ref);
+            CP = P * e_atual;
+            
+            
+            integral_atual = integral_anterior + (e_atual + e_anterior)*Ts/2;
+            integral_anterior = integral_atual;
+            
+            CI = P*I*integral_atual;
+            
+            derivada = (e_atual - e_anterior)/Ts;
+            CD = P*D*derivada;
+            
+            controlAction = CP+CI+CD;
+            if (controlAction >= 0.45){
+                controlAction  = 0.45;
+                }
+           
+            if (controlAction <= -0.45){
+                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.5 - controlAction);
+            motorDireita.write(0.5 + controlAction);
+            pc.printf("A[%4d]U[%.2f]\r\n",angle,controlAction);    
+         }
+          
+      }
+}
\ No newline at end of file