GERMAN REINA / HAL_QEI_PWM_MBED_PID

NUCLEO-F446RE ENCODER TIMER3 Y TIMER 2 HAL AND PWM. PID PARA UN PENDULO INVERTIDO CON 2 ENCODER DE CUADRATURA

Dependencies:   mbed-dev

main.cpp@0:95f924550159, 2016-05-21 (annotated)

Committer:
gereina
Date:
Sat May 21 04:17:35 2016 +0000
Revision:
0:95f924550159
QEI_TIMER2_TIMER_3 OK; PWM MBED; PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gereina 0:95f924550159 1 /*PROYECTO DEL PENDULO INVERTIDO
gereina 0:95f924550159 2 * TARJETA= NUCLEO STM32F446
gereina 0:95f924550159 3 ENCODER EJE X PB_8 CH1 PB_9 CH2
gereina 0:95f924550159 4 ENCODER PENDULO PB_4 PB_5
gereina 0:95f924550159 5
gereina 0:95f924550159 6 FIN DE CARRERA MOTOR IZQ = A1
gereina 0:95f924550159 7 FIN DERECHA DER = A0
gereina 0:95f924550159 8
gereina 0:95f924550159 9 POLOLULU
gereina 0:95f924550159 10
gereina 0:95f924550159 11 nD2 = D6
gereina 0:95f924550159 12 M1DIR=D7
gereina 0:95f924550159 13 M1PWM= D11 (TIMER 1)
gereina 0:95f924550159 14 NsF=D12
gereina 0:95f924550159 15 GND=D13
gereina 0:95f924550159 16 VCC=VCC
gereina 0:95f924550159 17 */
gereina 0:95f924550159 18
gereina 0:95f924550159 19
gereina 0:95f924550159 20
gereina 0:95f924550159 21 /*------------- THANKS DAVID LOWE FOR YOUR HELP-Your API/HAL use full ---------------------------------------
gereina 0:95f924550159 22 * Using STM32's counter peripherals to interface rotary encoders.
gereina 0:95f924550159 23 * Encoders are supported on F4xx's TIM1,2,3,4,5. TIM2 & TIM5 have 32bit count, others 16bit.
gereina 0:95f924550159 24 * Beware mbed uses TIM5 for system timer, SPI needs TIM1, others used for PWM.
gereina 0:95f924550159 25 * Check your platform's PeripheralPins.c & PeripheralNames.h if you need both PWM & encoders.
gereina 0:95f924550159 26 *
gereina 0:95f924550159 27 * Edit HAL_TIM_Encoder_MspInitFx.cpp to suit your mcu & board's available pinouts & pullups/downs.
gereina 0:95f924550159 28 *
gereina 0:95f924550159 29 * Thanks to:
gereina 0:95f924550159 30 * http://petoknm.wordpress.com/2015/01/05/rotary-encoder-and-stm32/
gereina 0:95f924550159 31 *
gereina 0:95f924550159 32 * References:
gereina 0:95f924550159 33 * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/user_manual/DM00122015.pdf
gereina 0:95f924550159 34 * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/reference_manual/DM00096844.pdf
gereina 0:95f924550159 35 * http://www.st.com/web/en/resource/technical/document/application_note/DM00042534.pdf
gereina 0:95f924550159 36 * http://www.st.com/web/en/resource/technical/document/datasheet/DM00102166.pdf
gereina 0:95f924550159 37 *
gereina 0:95f924550159 38 * David Lowe Jan 2015
gereina 0:95f924550159 39 *
gereina 0:95f924550159 40
gereina 0:95f924550159 41 *Se complementa con el Timer 8 en la tarjeta nucleo
gereina 0:95f924550159 42 *Efectivamente el Timer 5 no se debe usar.
gereina 0:95f924550159 43 *Se Ajustaron los pines para la tarjeta para usar otros para los PWM
gereina 0:95f924550159 44
gereina 0:95f924550159 45 */
gereina 0:95f924550159 46
gereina 0:95f924550159 47 #include "mbed.h"
gereina 0:95f924550159 48 #include "Encoder.h"
gereina 0:95f924550159 49
gereina 0:95f924550159 50
gereina 0:95f924550159 51 /****************************ENCODER INICIALIZACION *************************/
gereina 0:95f924550159 52 //STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h
gereina 0:95f924550159 53 #ifdef TARGET_STM32F3
gereina 0:95f924550159 54 #define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT)
gereina 0:95f924550159 55 #define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__) (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR))
gereina 0:95f924550159 56 #endif
gereina 0:95f924550159 57
gereina 0:95f924550159 58 TIM_Encoder_InitTypeDef encoder2, encoder3, encoder4,encoder5, encoder8;
gereina 0:95f924550159 59 TIM_HandleTypeDef timer2, timer3;
gereina 0:95f924550159 60
gereina 0:95f924550159 61 /*************************************************************************/
gereina 0:95f924550159 62
gereina 0:95f924550159 63 /*CONFIGURAMOS EL SHIELD DEL POLOLULU
gereina 0:95f924550159 64 Al coincidir con los pines del timer se modifica el codigo
gereina 0:95f924550159 65 */
gereina 0:95f924550159 66
gereina 0:95f924550159 67 DigitalOut M1DIR(D7);
gereina 0:95f924550159 68 PwmOut M1PWM(PA_7);
gereina 0:95f924550159 69 //AnalogIn M1FB(A0);
gereina 0:95f924550159 70
gereina 0:95f924550159 71 //DigitalOut M2DIR(D8);
gereina 0:95f924550159 72 //PwmOut M2PWM(D10);
gereina 0:95f924550159 73 //DigitalIn M2FB(A1);
gereina 0:95f924550159 74
gereina 0:95f924550159 75 DigitalOut nD2(D6,1);
gereina 0:95f924550159 76 DigitalIn nSF(D12);
gereina 0:95f924550159 77
gereina 0:95f924550159 78
gereina 0:95f924550159 79
gereina 0:95f924550159 80
gereina 0:95f924550159 81 /***********PUERTO SERIAL*****************/
gereina 0:95f924550159 82
gereina 0:95f924550159 83 Serial pc(SERIAL_TX, SERIAL_RX);
gereina 0:95f924550159 84
gereina 0:95f924550159 85
gereina 0:95f924550159 86 /*VALORES POR DEFECTO*/
gereina 0:95f924550159 87
gereina 0:95f924550159 88 float VEL=0.0;
gereina 0:95f924550159 89
gereina 0:95f924550159 90 /*VALORES DE CONFIGURACION DE LOS PINES*/
gereina 0:95f924550159 91
gereina 0:95f924550159 92 InterruptIn FINIZQR(A1);
gereina 0:95f924550159 93 InterruptIn FINDERM(A0);
gereina 0:95f924550159 94 //DigitalIn FINIZQR(D5);
gereina 0:95f924550159 95 //DigitalIn FINDERM(D6);
gereina 0:95f924550159 96
gereina 0:95f924550159 97 int alarma=0;
gereina 0:95f924550159 98 DigitalOut INDICA(LED2);
gereina 0:95f924550159 99 DigitalIn INIT(PC_13);
gereina 0:95f924550159 100
gereina 0:95f924550159 101
gereina 0:95f924550159 102 void toggle1(void);
gereina 0:95f924550159 103 void toggle2(void);
gereina 0:95f924550159 104 void inicializa(void);
gereina 0:95f924550159 105 void centrar(void);
gereina 0:95f924550159 106
gereina 0:95f924550159 107 /*-------------VARIABLES GLOBALES----------------------*/
gereina 0:95f924550159 108
gereina 0:95f924550159 109 float X=0;
gereina 0:95f924550159 110 float POS=0;
gereina 0:95f924550159 111 float A=0;
gereina 0:95f924550159 112 float ANG=0;
gereina 0:95f924550159 113 float LIMIN=300;//SI no inicializa toca manual 220
gereina 0:95f924550159 114 float LIMAX=7000; //Si no inciializa toca
gereina 0:95f924550159 115
gereina 0:95f924550159 116
gereina 0:95f924550159 117 float XMIN=0; //en las pruebas realizadas el 0 +200 pulsos
gereina 0:95f924550159 118 float XMAX=7200;; //en las pruebas aprox 7949
gereina 0:95f924550159 119 float MITAD=0;
gereina 0:95f924550159 120 int DATAPC=0;
gereina 0:95f924550159 121 int LEIDO=0;
gereina 0:95f924550159 122
gereina 0:95f924550159 123
gereina 0:95f924550159 124
gereina 0:95f924550159 125 /*------------------VARIABLES DEL PID Discreto--------------------------*/
gereina 0:95f924550159 126
gereina 0:95f924550159 127
gereina 0:95f924550159 128 float uc0,yn0,en0,cn0,uc1,yn1,cn1,en1,yn2,cn2,en2,yn3,cn3,en3;
gereina 0:95f924550159 129
gereina 0:95f924550159 130
gereina 0:95f924550159 131 //Constantes
gereina 0:95f924550159 132
gereina 0:95f924550159 133 float q0,q1,q2;
gereina 0:95f924550159 134
gereina 0:95f924550159 135 /*-----------------PID DE PENDULO COMPLETO-------------------------------*/
gereina 0:95f924550159 136 float PID_CODIGO(float MEDIDA,float SETPOINT,float DT);
gereina 0:95f924550159 137
gereina 0:95f924550159 138 float KI;
gereina 0:95f924550159 139 float KP;
gereina 0:95f924550159 140 float KD;
gereina 0:95f924550159 141 float ERR;
gereina 0:95f924550159 142 float PREVI_ERR;
gereina 0:95f924550159 143 float INTEGRAL;
gereina 0:95f924550159 144 float DERIVATIVO;
gereina 0:95f924550159 145
gereina 0:95f924550159 146 float DESEADO=180;
gereina 0:95f924550159 147
gereina 0:95f924550159 148 float TM=0.01; // tiempo de muestreo
gereina 0:95f924550159 149
gereina 0:95f924550159 150
gereina 0:95f924550159 151 float numero, cal_pwm;
gereina 0:95f924550159 152
gereina 0:95f924550159 153
gereina 0:95f924550159 154
gereina 0:95f924550159 155 /*-------------------------------------------------PROGRAMA---------------------------------------------------------------*/
gereina 0:95f924550159 156
gereina 0:95f924550159 157 int main()
gereina 0:95f924550159 158 {
gereina 0:95f924550159 159 float SALIDAPID;
gereina 0:95f924550159 160
gereina 0:95f924550159 161
gereina 0:95f924550159 162
gereina 0:95f924550159 163 //inicializacion de las varibles
gereina 0:95f924550159 164 yn0=0;
gereina 0:95f924550159 165 cn0=0;
gereina 0:95f924550159 166 en0=0;
gereina 0:95f924550159 167 yn1=0;
gereina 0:95f924550159 168 cn1=0;
gereina 0:95f924550159 169 en1=0;
gereina 0:95f924550159 170 yn2=0;
gereina 0:95f924550159 171 cn2=0;
gereina 0:95f924550159 172 en2=0;
gereina 0:95f924550159 173 yn3=0;
gereina 0:95f924550159 174 cn3=0;
gereina 0:95f924550159 175 en3=0;
gereina 0:95f924550159 176
gereina 0:95f924550159 177 q0=0.0510;
gereina 0:95f924550159 178 q1=-0.00102;
gereina 0:95f924550159 179 q2=0;
gereina 0:95f924550159 180
gereina 0:95f924550159 181 INTEGRAL=0.0;
gereina 0:95f924550159 182 KI=0.5;
gereina 0:95f924550159 183 KP=13;
gereina 0:95f924550159 184 KD=0.25;
gereina 0:95f924550159 185 SALIDAPID=0.0;
gereina 0:95f924550159 186
gereina 0:95f924550159 187
gereina 0:95f924550159 188
gereina 0:95f924550159 189
gereina 0:95f924550159 190 //Configuramos el puerto de 96000 a 115200 para mayor velocidad
gereina 0:95f924550159 191 pc.baud(115200);
gereina 0:95f924550159 192
gereina 0:95f924550159 193
gereina 0:95f924550159 194 //counting on both A&B inputs, 4 ticks per cycle, full 32-bit count
gereina 0:95f924550159 195 EncoderInit(&encoder2, &timer2, TIM2, 0xffffffff, TIM_ENCODERMODE_TI12);
gereina 0:95f924550159 196
gereina 0:95f924550159 197 //counting on both A&B inputs, 4 ticks per cycle, full 32-bit count
gereina 0:95f924550159 198 EncoderInit(&encoder3, &timer3, TIM3, 0xffff, TIM_ENCODERMODE_TI12);
gereina 0:95f924550159 199
gereina 0:95f924550159 200 int16_t count3=0;
gereina 0:95f924550159 201 int32_t count2=0;
gereina 0:95f924550159 202 //int8_t dir2, dir3;
gereina 0:95f924550159 203
gereina 0:95f924550159 204 M1PWM.period_ms(10.0); // set PWM period to 10 ms
gereina 0:95f924550159 205
gereina 0:95f924550159 206 pc.printf("Listo iniciamos centrando el carro !\n");
gereina 0:95f924550159 207 wait(0.1);
gereina 0:95f924550159 208
gereina 0:95f924550159 209
gereina 0:95f924550159 210 inicializa();
gereina 0:95f924550159 211 centrar();
gereina 0:95f924550159 212 alarma=1;
gereina 0:95f924550159 213
gereina 0:95f924550159 214
gereina 0:95f924550159 215
gereina 0:95f924550159 216 pc.printf("Centre el pendulo y Presione reset para continuar \n\r");
gereina 0:95f924550159 217 //pc.printf("Press 'a' = aumentar, 'd' = disminuir 'i'=Izquierda 'o'=derecha 'r'=reset contadores 's'=para solo el PWM \n\r");
gereina 0:95f924550159 218 char c ='s';
gereina 0:95f924550159 219 if (INIT==0){
gereina 0:95f924550159 220
gereina 0:95f924550159 221 yn0=0;
gereina 0:95f924550159 222 cn0=0;
gereina 0:95f924550159 223 en0=0;
gereina 0:95f924550159 224 yn1=0;
gereina 0:95f924550159 225 cn1=0;
gereina 0:95f924550159 226 en1=0;
gereina 0:95f924550159 227 yn2=0;
gereina 0:95f924550159 228 cn2=0;
gereina 0:95f924550159 229 en2=0;
gereina 0:95f924550159 230 yn3=0;
gereina 0:95f924550159 231 cn3=0;
gereina 0:95f924550159 232 en3=0;
gereina 0:95f924550159 233 alarma=0;
gereina 0:95f924550159 234 VEL=0;
gereina 0:95f924550159 235 numero=0;
gereina 0:95f924550159 236 wait(0.5);
gereina 0:95f924550159 237 }
gereina 0:95f924550159 238
gereina 0:95f924550159 239
gereina 0:95f924550159 240 //Ciclo principal
gereina 0:95f924550159 241 while(1) {
gereina 0:95f924550159 242
gereina 0:95f924550159 243
gereina 0:95f924550159 244 //----Deje habilitado la lectura del teclado para pruebas
gereina 0:95f924550159 245 if(pc.readable()==1) {
gereina 0:95f924550159 246 wait(0.1);
gereina 0:95f924550159 247 c = pc.getc();
gereina 0:95f924550159 248 pc.putc(c);
gereina 0:95f924550159 249
gereina 0:95f924550159 250
gereina 0:95f924550159 251 if((c == 'a') && (VEL < 0.99)) {
gereina 0:95f924550159 252 VEL += 0.01;
gereina 0:95f924550159 253 M1PWM = VEL;
gereina 0:95f924550159 254 }
gereina 0:95f924550159 255 if((c == 'd') && (VEL > 0.01)) {
gereina 0:95f924550159 256 VEL -= 0.01;
gereina 0:95f924550159 257 M1PWM = VEL;
gereina 0:95f924550159 258 }
gereina 0:95f924550159 259 if (c == 'i') {
gereina 0:95f924550159 260 M1DIR=1;
gereina 0:95f924550159 261 }
gereina 0:95f924550159 262 if (c == 'o') {
gereina 0:95f924550159 263 M1DIR=0;
gereina 0:95f924550159 264 }
gereina 0:95f924550159 265 if (c == 'r') {
gereina 0:95f924550159 266 __HAL_TIM_SetCounter(&timer3, 0x0000);
gereina 0:95f924550159 267 __HAL_TIM_SetCounter(&timer2, 0x00000000);
gereina 0:95f924550159 268 }
gereina 0:95f924550159 269 if (c == 's') {
gereina 0:95f924550159 270 M1PWM = 0.0;
gereina 0:95f924550159 271 }
gereina 0:95f924550159 272
gereina 0:95f924550159 273 c='0';
gereina 0:95f924550159 274 }
gereina 0:95f924550159 275 //----fin rutina de teclado
gereina 0:95f924550159 276
gereina 0:95f924550159 277 INDICA=alarma;
gereina 0:95f924550159 278
gereina 0:95f924550159 279 FINIZQR.rise(&toggle1);
gereina 0:95f924550159 280 FINDERM.rise(&toggle2);
gereina 0:95f924550159 281
gereina 0:95f924550159 282
gereina 0:95f924550159 283 //Rutina de lectura de los encoder timer2=X y Timer3 = Angulo
gereina 0:95f924550159 284 //X es de 100 huecos por revolucion = X4 400 pulsos por revolucion
gereina 0:95f924550159 285 //X es de 500 huecos por revolucuon = X4 2000 pulsos por revolucion
gereina 0:95f924550159 286 // X aproximado de 7950 +/10% por temas mecanicos de hay que sea variable el centro
gereina 0:95f924550159 287 // MITAD = 3757 depende de la ubicacion de los sensores ((MAX-220)-(MIN+220))/2
gereina 0:95f924550159 288
gereina 0:95f924550159 289 //count2=TIM2->CNT;
gereina 0:95f924550159 290 //dir2=TIM2->CR1&TIM_CR1_DIR;
gereina 0:95f924550159 291 count2=__HAL_TIM_GET_COUNTER(&timer2);
gereina 0:95f924550159 292 //dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); //no se necesita para el calculo
gereina 0:95f924550159 293
gereina 0:95f924550159 294 //count3=TIM3->CNT;
gereina 0:95f924550159 295 //dir3=TIM3->CR1&TIM_CR1_DIR;
gereina 0:95f924550159 296 count3=__HAL_TIM_GET_COUNTER(&timer3);
gereina 0:95f924550159 297 //dir3 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer3); //no se necesita para el calculo
gereina 0:95f924550159 298
gereina 0:95f924550159 299
gereina 0:95f924550159 300
gereina 0:95f924550159 301
gereina 0:95f924550159 302 X=count2;//sensor de posicion
gereina 0:95f924550159 303 A=count3;// sensor de angulo
gereina 0:95f924550159 304
gereina 0:95f924550159 305 pc.printf("%6.0f %6.0f \t", X,A);
gereina 0:95f924550159 306
gereina 0:95f924550159 307 if (INIT==0){
gereina 0:95f924550159 308
gereina 0:95f924550159 309 yn0=0;
gereina 0:95f924550159 310 cn0=0;
gereina 0:95f924550159 311 en0=0;
gereina 0:95f924550159 312 yn1=0;
gereina 0:95f924550159 313 cn1=0;
gereina 0:95f924550159 314 en1=0;
gereina 0:95f924550159 315 yn2=0;
gereina 0:95f924550159 316 cn2=0;
gereina 0:95f924550159 317 en2=0;
gereina 0:95f924550159 318 yn3=0;
gereina 0:95f924550159 319 cn3=0;
gereina 0:95f924550159 320 en3=0;
gereina 0:95f924550159 321 alarma=0;
gereina 0:95f924550159 322 VEL=0;
gereina 0:95f924550159 323 numero=0;
gereina 0:95f924550159 324 wait(0.5);
gereina 0:95f924550159 325 }
gereina 0:95f924550159 326
gereina 0:95f924550159 327
gereina 0:95f924550159 328 //factor de conversion por longitud/pulsos (720mm/7949 =0.090577 mm* pulso LTOTAL/PULSOS
gereina 0:95f924550159 329 POS=X*720/7949;
gereina 0:95f924550159 330
gereina 0:95f924550159 331
gereina 0:95f924550159 332 //factor de conversion grados vuelta/pulsos (360grados/2000 pulsos)=0.18 grados por pulso
gereina 0:95f924550159 333 ANG=A*0.18;
gereina 0:95f924550159 334
gereina 0:95f924550159 335 pc.printf("POS: %6.3f ANGULO:%6.3f \t", POS,ANG);
gereina 0:95f924550159 336
gereina 0:95f924550159 337
gereina 0:95f924550159 338 if(alarma==0){
gereina 0:95f924550159 339
gereina 0:95f924550159 340 SALIDAPID=PID_CODIGO(ANG, DESEADO, TM);
gereina 0:95f924550159 341
gereina 0:95f924550159 342 VEL=SALIDAPID;
gereina 0:95f924550159 343
gereina 0:95f924550159 344
gereina 0:95f924550159 345 //MAXIMO DE LA TARJETA
gereina 0:95f924550159 346
gereina 0:95f924550159 347 if (VEL>100) { //Salida PID si es mayor que el MAX
gereina 0:95f924550159 348 VEL=99;
gereina 0:95f924550159 349 }
gereina 0:95f924550159 350 if (VEL<-100) { //Salida PID si es mayor que el MAX
gereina 0:95f924550159 351 VEL=-99;
gereina 0:95f924550159 352 }
gereina 0:95f924550159 353
gereina 0:95f924550159 354 if((VEL<=100)&&(VEL>=0)){
gereina 0:95f924550159 355 VEL=VEL*1; //Error es positivo entonces gire a la izquierda lado opuesto del motor
gereina 0:95f924550159 356 M1DIR=0;
gereina 0:95f924550159 357 }
gereina 0:95f924550159 358
gereina 0:95f924550159 359 if((VEL<0)&&(VEL>-100)){
gereina 0:95f924550159 360 VEL=VEL*-1; //Error es negativo entonces gire a la derecha lado opuesto del motor
gereina 0:95f924550159 361 M1DIR=1;
gereina 0:95f924550159 362 }
gereina 0:95f924550159 363
gereina 0:95f924550159 364
gereina 0:95f924550159 365 if (abs(VEL)<10){ //Salida PID si es menor que el MIN
gereina 0:95f924550159 366 VEL=1;
gereina 0:95f924550159 367 }
gereina 0:95f924550159 368
gereina 0:95f924550159 369 M1PWM=(float)VEL/100;//para controlar con las teclas por silas moscas
gereina 0:95f924550159 370
gereina 0:95f924550159 371
gereina 0:95f924550159 372 }
gereina 0:95f924550159 373 pc.printf("PID = %f \t", SALIDAPID);
gereina 0:95f924550159 374 pc.printf("REF:%6.2f ANG:%6.2f ERROR: %6.2f ACC:%6.2f Itera: %.0f\t",DESEADO, ANG, ERR,VEL,numero);
gereina 0:95f924550159 375
gereina 0:95f924550159 376
gereina 0:95f924550159 377
gereina 0:95f924550159 378
gereina 0:95f924550159 379 //pc.printf("El PWM %1.3f \t ",VEL);
gereina 0:95f924550159 380 pc.printf("Alama: %i \n \r",alarma);
gereina 0:95f924550159 381
gereina 0:95f924550159 382
gereina 0:95f924550159 383
gereina 0:95f924550159 384
gereina 0:95f924550159 385
gereina 0:95f924550159 386 /*************************************************************************************************/
gereina 0:95f924550159 387 numero=numero+1;
gereina 0:95f924550159 388
gereina 0:95f924550159 389
gereina 0:95f924550159 390 /****seguridad***************/
gereina 0:95f924550159 391 if ((X+300)>=LIMAX){
gereina 0:95f924550159 392 VEL=0;
gereina 0:95f924550159 393 M1PWM=0;
gereina 0:95f924550159 394 alarma=1;
gereina 0:95f924550159 395 }
gereina 0:95f924550159 396 if ((X-300)<=LIMIN){
gereina 0:95f924550159 397 VEL=0.0;
gereina 0:95f924550159 398 M1PWM=0.0;
gereina 0:95f924550159 399 alarma=1;
gereina 0:95f924550159 400 }
gereina 0:95f924550159 401
gereina 0:95f924550159 402
gereina 0:95f924550159 403
gereina 0:95f924550159 404
gereina 0:95f924550159 405
gereina 0:95f924550159 406 }
gereina 0:95f924550159 407 }
gereina 0:95f924550159 408
gereina 0:95f924550159 409 /*---------------------FUNCIONES-----------------------------------------------*/
gereina 0:95f924550159 410
gereina 0:95f924550159 411 /*---------------------INTERUPPCIONES------------------------------------------*/
gereina 0:95f924550159 412
gereina 0:95f924550159 413 void toggle1() {
gereina 0:95f924550159 414 wait(0.25);
gereina 0:95f924550159 415 VEL = 0.0;
gereina 0:95f924550159 416 M1PWM = 0.0;
gereina 0:95f924550159 417 alarma=1;
gereina 0:95f924550159 418 pc.printf("ALARMA FINAL IZQUIERDA\n\r");
gereina 0:95f924550159 419 }
gereina 0:95f924550159 420
gereina 0:95f924550159 421 void toggle2() {
gereina 0:95f924550159 422 wait(0.25);
gereina 0:95f924550159 423 VEL = 0.0;
gereina 0:95f924550159 424 alarma=1;
gereina 0:95f924550159 425 M1PWM = 0.0;
gereina 0:95f924550159 426 pc.printf("ALARMA FINAL DERECHA\n\r");
gereina 0:95f924550159 427 }
gereina 0:95f924550159 428
gereina 0:95f924550159 429 /*--------------------------------FUCIONES DE INICIALIZACION------------------------------*/
gereina 0:95f924550159 430
gereina 0:95f924550159 431
gereina 0:95f924550159 432 void inicializa(){
gereina 0:95f924550159 433 pc.printf("Funcion de Inicializacion iniciada\n\r");
gereina 0:95f924550159 434
gereina 0:95f924550159 435 M1PWM=0.1;
gereina 0:95f924550159 436 M1DIR=1;
gereina 0:95f924550159 437 while(FINDERM==0){
gereina 0:95f924550159 438 pc.printf("BUSCANCO DERECHA DEL MOTOR\n\r");
gereina 0:95f924550159 439 }
gereina 0:95f924550159 440 pc.printf("ENCONTRADA - REINICIANDO EL CONTADOR EN X\n\r");
gereina 0:95f924550159 441 __HAL_TIM_SetCounter(&timer2, 0x00000000);
gereina 0:95f924550159 442
gereina 0:95f924550159 443 M1PWM=0.1;
gereina 0:95f924550159 444 M1DIR=0;
gereina 0:95f924550159 445
gereina 0:95f924550159 446 while(FINIZQR==0){
gereina 0:95f924550159 447 pc.printf("BUSCANCO IZQUIERDA LADO OPUESTO\n\r");
gereina 0:95f924550159 448 }
gereina 0:95f924550159 449 M1PWM=0;
gereina 0:95f924550159 450 XMAX=__HAL_TIM_GET_COUNTER(&timer2);
gereina 0:95f924550159 451 M1PWM=0;
gereina 0:95f924550159 452 XMIN=0;
gereina 0:95f924550159 453 LIMIN=220;
gereina 0:95f924550159 454 LIMAX=XMAX-220;
gereina 0:95f924550159 455 MITAD=(LIMAX-LIMIN)/2;
gereina 0:95f924550159 456 pc.printf("Fijando los limites XMIN+220 y XMAX-220 por seguridad, XMIN:%6.0f XMAX:%6.0f MITAD:%6.1f \n\r",XMIN, XMAX, MITAD);
gereina 0:95f924550159 457
gereina 0:95f924550159 458 }
gereina 0:95f924550159 459
gereina 0:95f924550159 460 void centrar(){
gereina 0:95f924550159 461 float error, pos;
gereina 0:95f924550159 462 int parada;
gereina 0:95f924550159 463 float itera;
gereina 0:95f924550159 464 parada=1;
gereina 0:95f924550159 465 itera=0;
gereina 0:95f924550159 466 pos=0;
gereina 0:95f924550159 467 error=0;
gereina 0:95f924550159 468
gereina 0:95f924550159 469 float K1, K2, K3, uk;
gereina 0:95f924550159 470
gereina 0:95f924550159 471 /*valores por defecto*/
gereina 0:95f924550159 472 //Kp=0.03;
gereina 0:95f924550159 473 //Ki=0;
gereina 0:95f924550159 474 //Kd=0;
gereina 0:95f924550159 475
gereina 0:95f924550159 476
gereina 0:95f924550159 477 /*parametros del PID discreto*/
gereina 0:95f924550159 478 // K1=Kp+(Ki/2)*Ts+(Kd/Ts);
gereina 0:95f924550159 479 // K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
gereina 0:95f924550159 480 // K3=Kd/Ts;
gereina 0:95f924550159 481
gereina 0:95f924550159 482 K1=0.19;
gereina 0:95f924550159 483 K2=0;
gereina 0:95f924550159 484 K3=0;
gereina 0:95f924550159 485
gereina 0:95f924550159 486
gereina 0:95f924550159 487
gereina 0:95f924550159 488 pos=__HAL_TIM_GET_COUNTER(&timer2);
gereina 0:95f924550159 489 error=MITAD-pos;
gereina 0:95f924550159 490
gereina 0:95f924550159 491 while(parada==1){
gereina 0:95f924550159 492
gereina 0:95f924550159 493 if (error>0){
gereina 0:95f924550159 494 //Error es positivo entonces gire a la izquierda lado opuesto del motor
gereina 0:95f924550159 495 M1DIR=0;
gereina 0:95f924550159 496 }
gereina 0:95f924550159 497 else if(error<=0){
gereina 0:95f924550159 498 //Error es negativo entonces gire a la derecha hacia el motor
gereina 0:95f924550159 499 M1DIR=1;
gereina 0:95f924550159 500 }
gereina 0:95f924550159 501
gereina 0:95f924550159 502 pos=__HAL_TIM_GET_COUNTER(&timer2);
gereina 0:95f924550159 503 error=MITAD-pos;
gereina 0:95f924550159 504
gereina 0:95f924550159 505 uk=error*K1;
gereina 0:95f924550159 506
gereina 0:95f924550159 507
gereina 0:95f924550159 508
gereina 0:95f924550159 509 pc.printf("BUSCA LA MITAD M:%6.0f P:%6.0f E:%6.1f Uk:%6.1f Itera:%6.0f\n\r",MITAD, pos, error,uk, itera);
gereina 0:95f924550159 510
gereina 0:95f924550159 511 if (abs(uk)>100) { //Salida si es mayor que el MAX
gereina 0:95f924550159 512 uk=80;}
gereina 0:95f924550159 513 else if (abs(uk)<20){ //Salida si es menor que el MIN
gereina 0:95f924550159 514 uk=5;
gereina 0:95f924550159 515 }
gereina 0:95f924550159 516
gereina 0:95f924550159 517 M1PWM=(float)uk/100; //Accion de control mapeada a PWM; salida de la tarjeta.
gereina 0:95f924550159 518
gereina 0:95f924550159 519
gereina 0:95f924550159 520
gereina 0:95f924550159 521 if (abs(error)<5){
gereina 0:95f924550159 522 parada=0;
gereina 0:95f924550159 523 }
gereina 0:95f924550159 524 if (itera>1000){
gereina 0:95f924550159 525 parada=0;
gereina 0:95f924550159 526 }
gereina 0:95f924550159 527
gereina 0:95f924550159 528 itera++;
gereina 0:95f924550159 529
gereina 0:95f924550159 530 }
gereina 0:95f924550159 531 M1PWM=0.0;
gereina 0:95f924550159 532 pc.printf("ENCONTRADO MITAD:%f POS:%f Error: %f Itera: %f\n\r",MITAD, pos, error, itera);
gereina 0:95f924550159 533
gereina 0:95f924550159 534
gereina 0:95f924550159 535 }
gereina 0:95f924550159 536
gereina 0:95f924550159 537 /******************************************PID*******************************************************/
gereina 0:95f924550159 538 float PID_CODIGO(float MEDIDA,float SETPOINT,float DT){
gereina 0:95f924550159 539 float OUTPID;
gereina 0:95f924550159 540
gereina 0:95f924550159 541 ERR=SETPOINT-MEDIDA;
gereina 0:95f924550159 542 INTEGRAL=INTEGRAL+ERR*DT;
gereina 0:95f924550159 543 DERIVATIVO=(ERR-PREVI_ERR)/DT;
gereina 0:95f924550159 544 OUTPID=KP*ERR-KI*INTEGRAL+KD*DERIVATIVO;
gereina 0:95f924550159 545 PREVI_ERR=ERR;
gereina 0:95f924550159 546
gereina 0:95f924550159 547 //wait(DT);
gereina 0:95f924550159 548 return(OUTPID);
gereina 0:95f924550159 549
gereina 0:95f924550159 550
gereina 0:95f924550159 551 //previous_error = 0
gereina 0:95f924550159 552 //integral = 0
gereina 0:95f924550159 553 //start:
gereina 0:95f924550159 554 //error = setpoint - measured_value
gereina 0:95f924550159 555 //integral = integral + error*dt
gereina 0:95f924550159 556 //derivative = (error - previous_error)/dt
gereina 0:95f924550159 557 //output = Kp*error + Ki*integral + Kd*derivative
gereina 0:95f924550159 558 //previous_error = error
gereina 0:95f924550159 559 //wait(dt)
gereina 0:95f924550159 560 //goto start
gereina 0:95f924550159 561
gereina 0:95f924550159 562
gereina 0:95f924550159 563
gereina 0:95f924550159 564 }