Revision 5:192f62832b17, committed 2019-12-03

Comitter:

helderoshiro

Date:

Tue Dec 03 04:01:35 2019 +0000

Parent:

4:7d74fc55c844

Commit message:

compilando;

Changed in this revision

--- a/main_copy.cpp	Tue Dec 03 03:59:09 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,176 +0,0 @@
-#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