Helder Oshiro / Mbed 2 deprecated Projeto-senoide

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Servo filter

main_copy.cpp@4:7d74fc55c844, 2019-12-03 (annotated)

Committer:
helderoshiro
Date:
Tue Dec 03 03:59:09 2019 +0000
Revision:
4:7d74fc55c844
Parent:
3:f850f53ff658 
print corrigido; angulo_media

Who changed what in which revision?

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