Helder Oshiro / Mbed 2 deprecated Projeto-senoide

senoide setpoint

Dependencies:   mbed Servo filter

main.cpp@1:3d924dfe29d6, 2019-11-27 (annotated)

Committer:
helderoshiro
Date:
Wed Nov 27 18:13:30 2019 +0000
Revision:
1:3d924dfe29d6
Parent:
0:802513e9e494
Child:
2:0d6718386397
media

Who changed what in which revision?

UserRevisionLine numberNew contents of line
helderoshiro 0:802513e9e494 1 #include "mbed.h"
helderoshiro 0:802513e9e494 2 #include "Servo.h"
helderoshiro 0:802513e9e494 3
helderoshiro 0:802513e9e494 4 /*****************
helderoshiro 0:802513e9e494 5 Código base para o controlador P.
helderoshiro 0:802513e9e494 6
helderoshiro 0:802513e9e494 7 O sistema fica no aguardo do usuário para começar.
helderoshiro 0:802513e9e494 8 Manda um cabeçalho na serial.
helderoshiro 0:802513e9e494 9 Depois ele pisca o led @ 5 Hz, 50 vezes.
helderoshiro 0:802513e9e494 10 Ele liga os motores em IDLE.
helderoshiro 0:802513e9e494 11 Manda um aviso de pronto.
helderoshiro 0:802513e9e494 12 Começa o loop de controle @ 50 Hz
helderoshiro 0:802513e9e494 13 - Le ADC ( Analogic Digital COnverter)
helderoshiro 0:802513e9e494 14 - Manda o valor lido pelo ad
helderoshiro 0:802513e9e494 15 *****************/
helderoshiro 0:802513e9e494 16
helderoshiro 0:802513e9e494 17 #define MOTOR_RESET_CONDITION 0.0f
helderoshiro 0:802513e9e494 18 #define MOTOR_IDLE_CONDITION 0.05f
helderoshiro 0:802513e9e494 19
helderoshiro 0:802513e9e494 20 //Não mexer - Definição dos Pinos da Bancada
helderoshiro 0:802513e9e494 21 DigitalOut myled(LED1);
helderoshiro 0:802513e9e494 22 InterruptIn button(USER_BUTTON);
helderoshiro 0:802513e9e494 23 Servo motorEsquerda(D5);
helderoshiro 0:802513e9e494 24 Servo motorDireita(D6);
helderoshiro 0:802513e9e494 25 AnalogIn encoder(PB_0);
helderoshiro 0:802513e9e494 26 Serial pc(SERIAL_TX, SERIAL_RX);
helderoshiro 0:802513e9e494 27 Ticker looper;
helderoshiro 0:802513e9e494 28
helderoshiro 0:802513e9e494 29 bool _btnFunction=false;
helderoshiro 0:802513e9e494 30 bool loopFlag = false;
helderoshiro 0:802513e9e494 31 float ref = 0.0f;
helderoshiro 0:802513e9e494 32 uint16_t angle;
helderoshiro 0:802513e9e494 33
helderoshiro 0:802513e9e494 34 void pressed()
helderoshiro 0:802513e9e494 35 {
helderoshiro 0:802513e9e494 36 _btnFunction = !_btnFunction;
helderoshiro 0:802513e9e494 37 }
helderoshiro 0:802513e9e494 38
helderoshiro 0:802513e9e494 39 void wait_user()
helderoshiro 0:802513e9e494 40 {
helderoshiro 0:802513e9e494 41 while(!_btnFunction) {
helderoshiro 0:802513e9e494 42 wait_ms(10);
helderoshiro 0:802513e9e494 43 }
helderoshiro 0:802513e9e494 44 _btnFunction = 0;
helderoshiro 0:802513e9e494 45 }
helderoshiro 0:802513e9e494 46
helderoshiro 0:802513e9e494 47 void blink_led(int period_in_ms, int times)
helderoshiro 0:802513e9e494 48 {
helderoshiro 0:802513e9e494 49 for(int i=0; times; i++) {
helderoshiro 0:802513e9e494 50 myled = !myled;
helderoshiro 0:802513e9e494 51 wait_ms(period_in_ms);
helderoshiro 0:802513e9e494 52 }
helderoshiro 0:802513e9e494 53 myled=0;
helderoshiro 0:802513e9e494 54 }
helderoshiro 0:802513e9e494 55
helderoshiro 0:802513e9e494 56 void loop()
helderoshiro 0:802513e9e494 57 {
helderoshiro 0:802513e9e494 58 loopFlag = true;
helderoshiro 0:802513e9e494 59 }
helderoshiro 0:802513e9e494 60
helderoshiro 0:802513e9e494 61 int main() {
helderoshiro 1:3d924dfe29d6 62 float janela[4] = {0, 0, 0, 0};
helderoshiro 1:3d924dfe29d6 63 float angulo_degree = 0;
helderoshiro 1:3d924dfe29d6 64 float angulo_media;
helderoshiro 0:802513e9e494 65 float P = 0.002; //v4 3*0.001;//3*0.00001;
helderoshiro 1:3d924dfe29d6 66 //float I = 1.6*5;//v4 1.6*50;
helderoshiro 1:3d924dfe29d6 67 //float D = 3.5/200;//v4 3.5/50;
helderoshiro 1:3d924dfe29d6 68 float I = 0.0002;//v4 1.6*50;
helderoshiro 1:3d924dfe29d6 69 float D = 0.0005;//v4 3.5/50;
helderoshiro 0:802513e9e494 70 float CP = 0;
helderoshiro 0:802513e9e494 71 float CI = 0;
helderoshiro 0:802513e9e494 72 float CD = 0;
helderoshiro 0:802513e9e494 73 float e_atual = 0;
helderoshiro 0:802513e9e494 74 float e_anterior = 0;
helderoshiro 0:802513e9e494 75 float integral_atual = 0;
helderoshiro 0:802513e9e494 76 float integral_anterior = 0;
helderoshiro 0:802513e9e494 77 float integral_temp = 0;
helderoshiro 0:802513e9e494 78 float Ts = 0.02;
helderoshiro 0:802513e9e494 79 float derivada = 0;
helderoshiro 0:802513e9e494 80 float angulo_V;
helderoshiro 0:802513e9e494 81 float angulo_d;
helderoshiro 0:802513e9e494 82 float controlAction = 0;
helderoshiro 0:802513e9e494 83
helderoshiro 0:802513e9e494 84 pc.baud(115200);
helderoshiro 0:802513e9e494 85 button.fall(&pressed);
helderoshiro 0:802513e9e494 86 motorEsquerda.write(MOTOR_RESET_CONDITION);
helderoshiro 0:802513e9e494 87 motorDireita.write(MOTOR_RESET_CONDITION);
helderoshiro 0:802513e9e494 88 myled = 1;
helderoshiro 0:802513e9e494 89 wait_user();
helderoshiro 0:802513e9e494 90 myled = 0;
helderoshiro 0:802513e9e494 91 pc.printf("Nucleo F401 turned on!\r\n");
helderoshiro 0:802513e9e494 92 //blink_led(20, 50);
helderoshiro 0:802513e9e494 93 motorDireita.write(MOTOR_IDLE_CONDITION);
helderoshiro 0:802513e9e494 94 motorEsquerda.write(MOTOR_IDLE_CONDITION);
helderoshiro 0:802513e9e494 95 pc.printf("Control should start in 10 s.\r\n");
helderoshiro 0:802513e9e494 96 wait(1);
helderoshiro 0:802513e9e494 97 looper.attach(&loop,0.02);
helderoshiro 0:802513e9e494 98 float sumError=0;
helderoshiro 0:802513e9e494 99 while(1)
helderoshiro 0:802513e9e494 100 {
helderoshiro 0:802513e9e494 101 if(loopFlag)
helderoshiro 0:802513e9e494 102 {
helderoshiro 0:802513e9e494 103 loopFlag = 0;
helderoshiro 1:3d924dfe29d6 104 int k = 0;
helderoshiro 1:3d924dfe29d6 105 while (k < 4){
helderoshiro 1:3d924dfe29d6 106 angle = encoder.read_u16()>>4; //Tensão em 12 bits
helderoshiro 1:3d924dfe29d6 107 /******
helderoshiro 1:3d924dfe29d6 108 Espaço reservado para colocar a ação de controle
helderoshiro 1:3d924dfe29d6 109 O exemplo é de um P
helderoshiro 1:3d924dfe29d6 110 ************/
helderoshiro 1:3d924dfe29d6 111 angulo_V = 0.000806*angle;
helderoshiro 1:3d924dfe29d6 112 angulo_degree = 27.779*angulo_V - 41.1;
helderoshiro 1:3d924dfe29d6 113 janela[k] = angulo_degree;
helderoshiro 1:3d924dfe29d6 114 k++;
helderoshiro 1:3d924dfe29d6 115 }
helderoshiro 1:3d924dfe29d6 116
helderoshiro 1:3d924dfe29d6 117 angulo_media = (janela[0]+janela[1]+janela[2]+janela[3])/4;
helderoshiro 0:802513e9e494 118 e_anterior = e_atual;
helderoshiro 1:3d924dfe29d6 119 e_atual = -(angulo_media - ref);
helderoshiro 0:802513e9e494 120 CP = P * e_atual;
helderoshiro 0:802513e9e494 121
helderoshiro 0:802513e9e494 122
helderoshiro 0:802513e9e494 123 integral_atual = integral_anterior + (e_atual + e_anterior)*Ts/2;
helderoshiro 0:802513e9e494 124 integral_anterior = integral_atual;
helderoshiro 0:802513e9e494 125
helderoshiro 1:3d924dfe29d6 126 //CI = P*I*integral_atual;
helderoshiro 1:3d924dfe29d6 127 CI = I*integral_atual;
helderoshiro 0:802513e9e494 128
helderoshiro 0:802513e9e494 129 derivada = (e_atual - e_anterior)/Ts;
helderoshiro 1:3d924dfe29d6 130 //CD = P*D*derivada;
helderoshiro 1:3d924dfe29d6 131 CD = D*derivada;
helderoshiro 0:802513e9e494 132
helderoshiro 0:802513e9e494 133 controlAction = CP+CI+CD;
helderoshiro 0:802513e9e494 134 if (controlAction >= 0.45){
helderoshiro 0:802513e9e494 135 controlAction = 0.45;
helderoshiro 0:802513e9e494 136 }
helderoshiro 0:802513e9e494 137
helderoshiro 0:802513e9e494 138 if (controlAction <= -0.45){
helderoshiro 0:802513e9e494 139 controlAction = -0.45;
helderoshiro 0:802513e9e494 140 }
helderoshiro 0:802513e9e494 141
helderoshiro 0:802513e9e494 142 //controlAction = 0.1*(ref - (float)angle);
helderoshiro 0:802513e9e494 143
helderoshiro 0:802513e9e494 144 /*
helderoshiro 0:802513e9e494 145 if(controlAction<0.95f) {
helderoshiro 0:802513e9e494 146 controlAction= 0.95f;
helderoshiro 0:802513e9e494 147 }
helderoshiro 0:802513e9e494 148 if(controlAction>0.0f) {
helderoshiro 0:802513e9e494 149 controlAction= 0.0f;
helderoshiro 0:802513e9e494 150 }*/
helderoshiro 0:802513e9e494 151
helderoshiro 1:3d924dfe29d6 152 motorEsquerda.write((0.8*(0.5 - controlAction)));
helderoshiro 0:802513e9e494 153 motorDireita.write(0.5 + controlAction);
helderoshiro 0:802513e9e494 154 pc.printf("A[%4d]U[%.2f]\r\n",angle,controlAction);
helderoshiro 0:802513e9e494 155 }
helderoshiro 0:802513e9e494 156
helderoshiro 0:802513e9e494 157 }
helderoshiro 0:802513e9e494 158 }