Helder Oshiro / Mbed 2 deprecated Projeto-senoide

senoide setpoint

Dependencies:   mbed Servo filter

main.cpp@3:f850f53ff658, 2019-12-03 (annotated)

Committer:
helderoshiro
Date:
Tue Dec 03 03:44:05 2019 +0000
Revision:
3:f850f53ff658
Parent:
2:0d6718386397
Child:
4:7d74fc55c844
setpoint;

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 2:0d6718386397 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 3:f850f53ff658 84 //Estou setando os dois setpoints aqui, serao os limiares que indicarao quando a referencia sera trocara e o movimento invertido
helderoshiro 3:f850f53ff658 85 float setpoint1 = 10;
helderoshiro 3:f850f53ff658 86 float setpoint2 = -setpoint1;
helderoshiro 3:f850f53ff658 87 bool subida = true;
helderoshiro 3:f850f53ff658 88
helderoshiro 3:f850f53ff658 89
helderoshiro 0:802513e9e494 90 pc.baud(115200);
helderoshiro 0:802513e9e494 91 button.fall(&pressed);
helderoshiro 0:802513e9e494 92 motorEsquerda.write(MOTOR_RESET_CONDITION);
helderoshiro 0:802513e9e494 93 motorDireita.write(MOTOR_RESET_CONDITION);
helderoshiro 0:802513e9e494 94 myled = 1;
helderoshiro 0:802513e9e494 95 wait_user();
helderoshiro 0:802513e9e494 96 myled = 0;
helderoshiro 0:802513e9e494 97 pc.printf("Nucleo F401 turned on!\r\n");
helderoshiro 0:802513e9e494 98 //blink_led(20, 50);
helderoshiro 0:802513e9e494 99 motorDireita.write(MOTOR_IDLE_CONDITION);
helderoshiro 0:802513e9e494 100 motorEsquerda.write(MOTOR_IDLE_CONDITION);
helderoshiro 0:802513e9e494 101 pc.printf("Control should start in 10 s.\r\n");
helderoshiro 0:802513e9e494 102 wait(1);
helderoshiro 0:802513e9e494 103 looper.attach(&loop,0.02);
helderoshiro 0:802513e9e494 104 float sumError=0;
helderoshiro 0:802513e9e494 105 while(1)
helderoshiro 0:802513e9e494 106 {
helderoshiro 0:802513e9e494 107 if(loopFlag)
helderoshiro 0:802513e9e494 108 {
helderoshiro 0:802513e9e494 109 loopFlag = 0;
helderoshiro 3:f850f53ff658 110
helderoshiro 3:f850f53ff658 111 //DEFINICAO DO SETPOINT - subida ou descida
helderoshiro 3:f850f53ff658 112 if (subida){
helderoshiro 3:f850f53ff658 113 ref = setpoint1;
helderoshiro 3:f850f53ff658 114 }
helderoshiro 3:f850f53ff658 115 else {
helderoshiro 3:f850f53ff658 116 ref = setpoint2;
helderoshiro 3:f850f53ff658 117 }
helderoshiro 3:f850f53ff658 118
helderoshiro 3:f850f53ff658 119
helderoshiro 1:3d924dfe29d6 120 int k = 0;
helderoshiro 2:0d6718386397 121 janela = 0;
helderoshiro 1:3d924dfe29d6 122 while (k < 4){
helderoshiro 1:3d924dfe29d6 123 angle = encoder.read_u16()>>4; //Tensão em 12 bits
helderoshiro 1:3d924dfe29d6 124 /******
helderoshiro 1:3d924dfe29d6 125 Espaço reservado para colocar a ação de controle
helderoshiro 1:3d924dfe29d6 126 O exemplo é de um P
helderoshiro 1:3d924dfe29d6 127 ************/
helderoshiro 1:3d924dfe29d6 128 angulo_V = 0.000806*angle;
helderoshiro 1:3d924dfe29d6 129 angulo_degree = 27.779*angulo_V - 41.1;
helderoshiro 2:0d6718386397 130 janela = janela + angulo_degree;
helderoshiro 1:3d924dfe29d6 131 k++;
helderoshiro 1:3d924dfe29d6 132 }
helderoshiro 1:3d924dfe29d6 133
helderoshiro 2:0d6718386397 134 angulo_media = janela/k; //(janela[0]+janela[1]+janela[2]+janela[3])/4;
helderoshiro 0:802513e9e494 135 e_anterior = e_atual;
helderoshiro 1:3d924dfe29d6 136 e_atual = -(angulo_media - ref);
helderoshiro 0:802513e9e494 137 CP = P * e_atual;
helderoshiro 0:802513e9e494 138
helderoshiro 0:802513e9e494 139
helderoshiro 0:802513e9e494 140 integral_atual = integral_anterior + (e_atual + e_anterior)*Ts/2;
helderoshiro 0:802513e9e494 141 integral_anterior = integral_atual;
helderoshiro 0:802513e9e494 142
helderoshiro 1:3d924dfe29d6 143 //CI = P*I*integral_atual;
helderoshiro 1:3d924dfe29d6 144 CI = I*integral_atual;
helderoshiro 0:802513e9e494 145
helderoshiro 0:802513e9e494 146 derivada = (e_atual - e_anterior)/Ts;
helderoshiro 1:3d924dfe29d6 147 //CD = P*D*derivada;
helderoshiro 1:3d924dfe29d6 148 CD = D*derivada;
helderoshiro 0:802513e9e494 149
helderoshiro 0:802513e9e494 150 controlAction = CP+CI+CD;
helderoshiro 0:802513e9e494 151 if (controlAction >= 0.45){
helderoshiro 0:802513e9e494 152 controlAction = 0.45;
helderoshiro 0:802513e9e494 153 }
helderoshiro 0:802513e9e494 154
helderoshiro 0:802513e9e494 155 if (controlAction <= -0.45){
helderoshiro 0:802513e9e494 156 controlAction = -0.45;
helderoshiro 0:802513e9e494 157 }
helderoshiro 0:802513e9e494 158
helderoshiro 0:802513e9e494 159
helderoshiro 1:3d924dfe29d6 160 motorEsquerda.write((0.8*(0.5 - controlAction)));
helderoshiro 0:802513e9e494 161 motorDireita.write(0.5 + controlAction);
helderoshiro 3:f850f53ff658 162 pc.printf("A[%4d]U[%.2f]\r\n",angle_media,controlAction);
helderoshiro 3:f850f53ff658 163
helderoshiro 3:f850f53ff658 164
helderoshiro 3:f850f53ff658 165 //Se ele estiver subindo, mas subiu mais que o setpoint1, entao deve descer
helderoshiro 3:f850f53ff658 166 //Se estiver descendo, mas desceu mais que o setpoint2, entao deve subir
helderoshiro 3:f850f53ff658 167 if (subida && angulo_media>setpoint1){
helderoshiro 3:f850f53ff658 168 subida = false;
helderoshiro 3:f850f53ff658 169 }
helderoshiro 3:f850f53ff658 170 if (!(subida) && angulo_media<setpoint2){
helderoshiro 3:f850f53ff658 171 subida = true;
helderoshiro 3:f850f53ff658 172 }
helderoshiro 0:802513e9e494 173 }
helderoshiro 0:802513e9e494 174
helderoshiro 0:802513e9e494 175 }
helderoshiro 0:802513e9e494 176 }