NUCLEO-F446RE ENCODER TIMER3 Y TIMER 2 HAL AND PWM. PID PARA UN PENDULO INVERTIDO CON 2 ENCODER DE CUADRATURA
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?
User | Revision | Line number | New 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 | } |