David Cordoba
/
P2_ControlPID_Proc20191
Entrega numero 2 del curso procesadores 2019-2
main.cpp@0:55be7cc4ef10, 2019-06-14 (annotated)
- Committer:
- diavad
- Date:
- Fri Jun 14 20:50:38 2019 +0000
- Revision:
- 0:55be7cc4ef10
Entrega numero 2 procesadores 20191
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
diavad | 0:55be7cc4ef10 | 1 | #include "mbed.h" |
diavad | 0:55be7cc4ef10 | 2 | #include <stdio.h> |
diavad | 0:55be7cc4ef10 | 3 | #include <string.h> |
diavad | 0:55be7cc4ef10 | 4 | #include <stdlib.h> |
diavad | 0:55be7cc4ef10 | 5 | |
diavad | 0:55be7cc4ef10 | 6 | #define BT_RX PTE1 |
diavad | 0:55be7cc4ef10 | 7 | #define BT_TX PTE0 |
diavad | 0:55be7cc4ef10 | 8 | |
diavad | 0:55be7cc4ef10 | 9 | /*Alvaro Javier Vargas Miranda |
diavad | 0:55be7cc4ef10 | 10 | Guillermo Luna Rozo |
diavad | 0:55be7cc4ef10 | 11 | Santiago Yepes Montoya |
diavad | 0:55be7cc4ef10 | 12 | Juan David Cordoba Giraldo |
diavad | 0:55be7cc4ef10 | 13 | Susana Buriticá Mejía*/ |
diavad | 0:55be7cc4ef10 | 14 | |
diavad | 0:55be7cc4ef10 | 15 | DigitalOut lRed(LED1); |
diavad | 0:55be7cc4ef10 | 16 | DigitalOut lGrn(LED2); |
diavad | 0:55be7cc4ef10 | 17 | DigitalOut lBlu(LED3); |
diavad | 0:55be7cc4ef10 | 18 | |
diavad | 0:55be7cc4ef10 | 19 | AnalogIn AnInput(PTB0); |
diavad | 0:55be7cc4ef10 | 20 | AnalogOut AnOut(PTE30); |
diavad | 0:55be7cc4ef10 | 21 | PwmOut pwmOut(PTE20); |
diavad | 0:55be7cc4ef10 | 22 | |
diavad | 0:55be7cc4ef10 | 23 | Serial PC(USBTX, USBRX); // tx, rx |
diavad | 0:55be7cc4ef10 | 24 | Serial BT(BT_TX, BT_RX); // Serial para el Bluetooth |
diavad | 0:55be7cc4ef10 | 25 | |
diavad | 0:55be7cc4ef10 | 26 | /* Variables Globales */ |
diavad | 0:55be7cc4ef10 | 27 | // Parametros del PID sintonizado |
diavad | 0:55be7cc4ef10 | 28 | float cKp=1.432f; |
diavad | 0:55be7cc4ef10 | 29 | float cKi=25.95f; |
diavad | 0:55be7cc4ef10 | 30 | float cKd=0.0f; |
diavad | 0:55be7cc4ef10 | 31 | float cRf=1.0f; |
diavad | 0:55be7cc4ef10 | 32 | float out,ai,ad,ap,med,err,err_v,uPID; |
diavad | 0:55be7cc4ef10 | 33 | int Ts=250; |
diavad | 0:55be7cc4ef10 | 34 | bool continuo = true; |
diavad | 0:55be7cc4ef10 | 35 | |
diavad | 0:55be7cc4ef10 | 36 | char buffer[5]; //Buffer para almacenar los msjs del usuario |
diavad | 0:55be7cc4ef10 | 37 | char dbg[100]=""; |
diavad | 0:55be7cc4ef10 | 38 | |
diavad | 0:55be7cc4ef10 | 39 | void cleanBuffer(char *buffer) |
diavad | 0:55be7cc4ef10 | 40 | { |
diavad | 0:55be7cc4ef10 | 41 | for(int i=0; i< 5; i++) { |
diavad | 0:55be7cc4ef10 | 42 | buffer[i]='\0'; |
diavad | 0:55be7cc4ef10 | 43 | } |
diavad | 0:55be7cc4ef10 | 44 | } |
diavad | 0:55be7cc4ef10 | 45 | |
diavad | 0:55be7cc4ef10 | 46 | void printMenuBT(void) |
diavad | 0:55be7cc4ef10 | 47 | { |
diavad | 0:55be7cc4ef10 | 48 | char Menu1[] = "|********** CONTROLADOR PID ***********|\n"; |
diavad | 0:55be7cc4ef10 | 49 | char Menu2[] = "|* *|\n"; |
diavad | 0:55be7cc4ef10 | 50 | char Menu3[] = "|( P ) -- Ingresar parametros del PID *|\n"; |
diavad | 0:55be7cc4ef10 | 51 | char Menu4[] = "|( I ) -- PID con DCA *|\n"; |
diavad | 0:55be7cc4ef10 | 52 | char Menu5[] = "|( W ) -- PID con PWM *|\n"; |
diavad | 0:55be7cc4ef10 | 53 | char Menu6[] = "|( S ) -- Apagar la salida *|\n"; |
diavad | 0:55be7cc4ef10 | 54 | char Menu7[] = "|* *|\n"; |
diavad | 0:55be7cc4ef10 | 55 | char Menu8[] = "|**************************************|\n"; |
diavad | 0:55be7cc4ef10 | 56 | |
diavad | 0:55be7cc4ef10 | 57 | //Impresion del Menu |
diavad | 0:55be7cc4ef10 | 58 | |
diavad | 0:55be7cc4ef10 | 59 | BT.printf("\n%s",Menu1); |
diavad | 0:55be7cc4ef10 | 60 | BT.printf("%s",Menu2); |
diavad | 0:55be7cc4ef10 | 61 | BT.printf("%s",Menu3); |
diavad | 0:55be7cc4ef10 | 62 | BT.printf("%s",Menu4); |
diavad | 0:55be7cc4ef10 | 63 | BT.printf("%s",Menu5); |
diavad | 0:55be7cc4ef10 | 64 | BT.printf("%s",Menu6); |
diavad | 0:55be7cc4ef10 | 65 | BT.printf("%s",Menu7); |
diavad | 0:55be7cc4ef10 | 66 | BT.printf("%s",Menu8); |
diavad | 0:55be7cc4ef10 | 67 | } |
diavad | 0:55be7cc4ef10 | 68 | |
diavad | 0:55be7cc4ef10 | 69 | void enterPIDParameters(void) |
diavad | 0:55be7cc4ef10 | 70 | { |
diavad | 0:55be7cc4ef10 | 71 | bool ready=false; |
diavad | 0:55be7cc4ef10 | 72 | float kp,ki,kd; |
diavad | 0:55be7cc4ef10 | 73 | float ref; |
diavad | 0:55be7cc4ef10 | 74 | char ans[5]; |
diavad | 0:55be7cc4ef10 | 75 | |
diavad | 0:55be7cc4ef10 | 76 | BT.printf("Ingrese los parametros del controlador.\n\r"); |
diavad | 0:55be7cc4ef10 | 77 | while(ready == false) |
diavad | 0:55be7cc4ef10 | 78 | { |
diavad | 0:55be7cc4ef10 | 79 | BT.printf("PID valores actuales\nKp=%1.4f ki=%1.4f kd=%1.4f Ref=%1.4f\n",cKp,cKi,cKd,cRf); |
diavad | 0:55be7cc4ef10 | 80 | BT.printf("Kp >> "); |
diavad | 0:55be7cc4ef10 | 81 | BT.scanf("%f",&kp); |
diavad | 0:55be7cc4ef10 | 82 | BT.printf("Ki >> "); |
diavad | 0:55be7cc4ef10 | 83 | BT.scanf("%f",&ki); |
diavad | 0:55be7cc4ef10 | 84 | BT.printf("Kd >> "); |
diavad | 0:55be7cc4ef10 | 85 | BT.scanf("%f",&kd); |
diavad | 0:55be7cc4ef10 | 86 | BT.printf("Ref [0 - 3.3]>> "); |
diavad | 0:55be7cc4ef10 | 87 | BT.scanf("%f",&ref); |
diavad | 0:55be7cc4ef10 | 88 | |
diavad | 0:55be7cc4ef10 | 89 | BT.printf("Nuevos Valores\nKp=%1.4f Ki=%1.4f Kd=%1.4f Ref=%1.4f\n",kp,ki,kd,ref); |
diavad | 0:55be7cc4ef10 | 90 | BT.printf("Desea continuar? (S/n), (Q) para salir "); |
diavad | 0:55be7cc4ef10 | 91 | BT.scanf("%s",ans); |
diavad | 0:55be7cc4ef10 | 92 | PC.printf("Llego %s \n",ans); |
diavad | 0:55be7cc4ef10 | 93 | |
diavad | 0:55be7cc4ef10 | 94 | if(strncmp(ans,"S",1)==0 || strncmp(ans,"s",1)==0 || strncmp(ans,"q",1)==0 ||strncmp(ans,"Q",1)==0) { |
diavad | 0:55be7cc4ef10 | 95 | PC.printf("Entre\n"); |
diavad | 0:55be7cc4ef10 | 96 | ready = true; |
diavad | 0:55be7cc4ef10 | 97 | } |
diavad | 0:55be7cc4ef10 | 98 | } |
diavad | 0:55be7cc4ef10 | 99 | if(strncmp(ans,"S",1)==0 || strncmp(ans,"s",1)==0){ |
diavad | 0:55be7cc4ef10 | 100 | cKp = kp; |
diavad | 0:55be7cc4ef10 | 101 | cKi = ki; |
diavad | 0:55be7cc4ef10 | 102 | cKd = kd; |
diavad | 0:55be7cc4ef10 | 103 | cRf = ref; |
diavad | 0:55be7cc4ef10 | 104 | lRed=0; |
diavad | 0:55be7cc4ef10 | 105 | lGrn=0; |
diavad | 0:55be7cc4ef10 | 106 | BT.printf("PID valores\nKp=%1.2f ki=%1.2f kd=%1.2f Ref=%1.2f\n",cKp,cKi,cKd,cRf); |
diavad | 0:55be7cc4ef10 | 107 | } |
diavad | 0:55be7cc4ef10 | 108 | |
diavad | 0:55be7cc4ef10 | 109 | } |
diavad | 0:55be7cc4ef10 | 110 | |
diavad | 0:55be7cc4ef10 | 111 | // CICLO PRINCIPAL CONTROLADOR PID |
diavad | 0:55be7cc4ef10 | 112 | |
diavad | 0:55be7cc4ef10 | 113 | void controlPID(){ |
diavad | 0:55be7cc4ef10 | 114 | med = AnInput.read()*999; //Medicion de la salida |
diavad | 0:55be7cc4ef10 | 115 | err = cRf*(999/3.3f) - med; //calculo del error |
diavad | 0:55be7cc4ef10 | 116 | ap = err*cKp*0.01f; //Calculo accion proporcional |
diavad | 0:55be7cc4ef10 | 117 | ai = ai+(err*cKi*0.01f); //Calculo accion integral |
diavad | 0:55be7cc4ef10 | 118 | ad = (err-err_v)*(cKd)*0.01f;//Calculo de la accion derivativa |
diavad | 0:55be7cc4ef10 | 119 | uPID=ap+ai+ad; //Calculo de la accion de control |
diavad | 0:55be7cc4ef10 | 120 | if(uPID<=0){uPID=0;} //Saturador inferior |
diavad | 0:55be7cc4ef10 | 121 | if(uPID>=999){uPID=999;} //Saturador superior |
diavad | 0:55be7cc4ef10 | 122 | err_v = err; //Actualizacion de error |
diavad | 0:55be7cc4ef10 | 123 | out=uPID*(3.3f/999); //Conversion de voltaje a accion de control |
diavad | 0:55be7cc4ef10 | 124 | AnOut.write(out); //Escritura de accion de control |
diavad | 0:55be7cc4ef10 | 125 | BT.printf("Ref=%fv Error=%fv Ucontrol=%fv\n\r",cRf,err/999,out); |
diavad | 0:55be7cc4ef10 | 126 | } |
diavad | 0:55be7cc4ef10 | 127 | |
diavad | 0:55be7cc4ef10 | 128 | void controlPIDpwm(){ |
diavad | 0:55be7cc4ef10 | 129 | med = AnInput.read()*999; //Medicion de la salida |
diavad | 0:55be7cc4ef10 | 130 | err = cRf*(999/3.3f) - med; //calculo del error |
diavad | 0:55be7cc4ef10 | 131 | ap = err*cKp*0.01f; //Calculo accion proporcional |
diavad | 0:55be7cc4ef10 | 132 | ai = ai+(err*cKi*0.01f); //Calculo accion integral |
diavad | 0:55be7cc4ef10 | 133 | ad = (err-err_v)*(cKd)*0.01f;//Calculo de la accion derivativa |
diavad | 0:55be7cc4ef10 | 134 | uPID=ap+ai+ad; //Calculo de la accion de control |
diavad | 0:55be7cc4ef10 | 135 | if(uPID<=0){uPID=0;} //Saturador inferior |
diavad | 0:55be7cc4ef10 | 136 | if(uPID>=999){uPID=999;} //Saturador superior |
diavad | 0:55be7cc4ef10 | 137 | err_v = err; //Actualizacion de error |
diavad | 0:55be7cc4ef10 | 138 | out=uPID*(3.3f/999); //Conversion de voltaje a accion de control |
diavad | 0:55be7cc4ef10 | 139 | pwmOut.write(out); //Escritura de accion de control |
diavad | 0:55be7cc4ef10 | 140 | BT.printf("Ref=%fv Error=%fv Ucontrol=%fv\n\r",cRf,err/999,out); |
diavad | 0:55be7cc4ef10 | 141 | } |
diavad | 0:55be7cc4ef10 | 142 | |
diavad | 0:55be7cc4ef10 | 143 | |
diavad | 0:55be7cc4ef10 | 144 | void LeerBuffer(char *buffer) |
diavad | 0:55be7cc4ef10 | 145 | { |
diavad | 0:55be7cc4ef10 | 146 | int i= 0; |
diavad | 0:55be7cc4ef10 | 147 | while (BT.readable()) { |
diavad | 0:55be7cc4ef10 | 148 | char c = BT.getc(); |
diavad | 0:55be7cc4ef10 | 149 | if (c == '\r' || c == '\n' || c == '\t') c = '$';//si se envia fin de linea o de caracter inserta $ |
diavad | 0:55be7cc4ef10 | 150 | buffer[i++] = c;//mete al bufer el caracter leido |
diavad | 0:55be7cc4ef10 | 151 | } |
diavad | 0:55be7cc4ef10 | 152 | } |
diavad | 0:55be7cc4ef10 | 153 | |
diavad | 0:55be7cc4ef10 | 154 | |
diavad | 0:55be7cc4ef10 | 155 | void executePIDControl() |
diavad | 0:55be7cc4ef10 | 156 | { |
diavad | 0:55be7cc4ef10 | 157 | PC.printf("Entre al PID\n"); |
diavad | 0:55be7cc4ef10 | 158 | //Capturamos los parametros del controlador |
diavad | 0:55be7cc4ef10 | 159 | bool run=true; |
diavad | 0:55be7cc4ef10 | 160 | out=0;ai=0;ad=0;ap=0;med=0;err=0;err_v=0;uPID=0; |
diavad | 0:55be7cc4ef10 | 161 | while(run == true) { |
diavad | 0:55be7cc4ef10 | 162 | lRed=1; |
diavad | 0:55be7cc4ef10 | 163 | lGrn=0; |
diavad | 0:55be7cc4ef10 | 164 | if(continuo){controlPID();} |
diavad | 0:55be7cc4ef10 | 165 | else{controlPIDpwm();} |
diavad | 0:55be7cc4ef10 | 166 | wait_ms(Ts); |
diavad | 0:55be7cc4ef10 | 167 | LeerBuffer(buffer); |
diavad | 0:55be7cc4ef10 | 168 | if(buffer[0]=='d' || buffer[0]=='D' || buffer[1]=='d' || buffer[1]=='D') { |
diavad | 0:55be7cc4ef10 | 169 | run = false; |
diavad | 0:55be7cc4ef10 | 170 | } |
diavad | 0:55be7cc4ef10 | 171 | cleanBuffer(buffer); |
diavad | 0:55be7cc4ef10 | 172 | } |
diavad | 0:55be7cc4ef10 | 173 | } |
diavad | 0:55be7cc4ef10 | 174 | |
diavad | 0:55be7cc4ef10 | 175 | |
diavad | 0:55be7cc4ef10 | 176 | int main() |
diavad | 0:55be7cc4ef10 | 177 | { |
diavad | 0:55be7cc4ef10 | 178 | BT.baud(9600); |
diavad | 0:55be7cc4ef10 | 179 | AnOut.write(0.0f); |
diavad | 0:55be7cc4ef10 | 180 | pwmOut.write(0.0f); |
diavad | 0:55be7cc4ef10 | 181 | lRed = 1; |
diavad | 0:55be7cc4ef10 | 182 | lBlu = 1; |
diavad | 0:55be7cc4ef10 | 183 | for(int i = 0; i<3; i++) { |
diavad | 0:55be7cc4ef10 | 184 | /* Parpadeo de LEDS para indicar el inicio*/ |
diavad | 0:55be7cc4ef10 | 185 | lGrn = 0; |
diavad | 0:55be7cc4ef10 | 186 | wait(0.25); |
diavad | 0:55be7cc4ef10 | 187 | lGrn = 1; |
diavad | 0:55be7cc4ef10 | 188 | wait(0.25); |
diavad | 0:55be7cc4ef10 | 189 | } |
diavad | 0:55be7cc4ef10 | 190 | PC.printf("\nProgram Start!\n"); |
diavad | 0:55be7cc4ef10 | 191 | |
diavad | 0:55be7cc4ef10 | 192 | /*Led en naranja para indicar que esta listo para recibir parametros*/ |
diavad | 0:55be7cc4ef10 | 193 | lRed = 0; |
diavad | 0:55be7cc4ef10 | 194 | lGrn = 1; |
diavad | 0:55be7cc4ef10 | 195 | enterPIDParameters(); |
diavad | 0:55be7cc4ef10 | 196 | |
diavad | 0:55be7cc4ef10 | 197 | while(1) { |
diavad | 0:55be7cc4ef10 | 198 | printMenuBT(); |
diavad | 0:55be7cc4ef10 | 199 | BT.printf("\nIngrese una opcion >> "); |
diavad | 0:55be7cc4ef10 | 200 | BT.scanf("%s",buffer); |
diavad | 0:55be7cc4ef10 | 201 | PC.printf("\n Se recibio: %s \n",buffer); |
diavad | 0:55be7cc4ef10 | 202 | |
diavad | 0:55be7cc4ef10 | 203 | char msg[10]; |
diavad | 0:55be7cc4ef10 | 204 | int mIdx = 0; |
diavad | 0:55be7cc4ef10 | 205 | for (int m = 0; m < 10; m++) { |
diavad | 0:55be7cc4ef10 | 206 | if (buffer[m] != '\n' && buffer[m] != '\r' && buffer[m] != ' ') { |
diavad | 0:55be7cc4ef10 | 207 | msg[mIdx] = buffer[m]; //Guardar los caracteres validos en el mensaje |
diavad | 0:55be7cc4ef10 | 208 | mIdx++; //Corremos el indice del array de chars |
diavad | 0:55be7cc4ef10 | 209 | } |
diavad | 0:55be7cc4ef10 | 210 | } |
diavad | 0:55be7cc4ef10 | 211 | |
diavad | 0:55be7cc4ef10 | 212 | PC.printf("Msg[1] es <%s> \n",msg); |
diavad | 0:55be7cc4ef10 | 213 | if(strncmp(msg,"P",1)==0 || strncmp(msg,"p",1)==0){ |
diavad | 0:55be7cc4ef10 | 214 | enterPIDParameters(); |
diavad | 0:55be7cc4ef10 | 215 | BT.printf("Se han ingreso los parametros del PID\n"); |
diavad | 0:55be7cc4ef10 | 216 | } |
diavad | 0:55be7cc4ef10 | 217 | else if(strncmp(msg,"i",1)==0 || strncmp(msg,"I",1)==0){ |
diavad | 0:55be7cc4ef10 | 218 | PC.printf("Llame al PID continuo\n"); |
diavad | 0:55be7cc4ef10 | 219 | continuo=true; |
diavad | 0:55be7cc4ef10 | 220 | pwmOut.write(0.0f); |
diavad | 0:55be7cc4ef10 | 221 | executePIDControl(); |
diavad | 0:55be7cc4ef10 | 222 | lRed=0; |
diavad | 0:55be7cc4ef10 | 223 | lGrn=0; |
diavad | 0:55be7cc4ef10 | 224 | //BT.printf("Se han ingreso los parametros del PID\n"); |
diavad | 0:55be7cc4ef10 | 225 | } |
diavad | 0:55be7cc4ef10 | 226 | else if(strncmp(msg,"w",1)==0 || strncmp(msg,"W",1)==0){ |
diavad | 0:55be7cc4ef10 | 227 | PC.printf("Llame al PID pwm\n"); |
diavad | 0:55be7cc4ef10 | 228 | continuo=false; |
diavad | 0:55be7cc4ef10 | 229 | AnOut.write(0.0f); |
diavad | 0:55be7cc4ef10 | 230 | executePIDControl(); |
diavad | 0:55be7cc4ef10 | 231 | lRed=0; |
diavad | 0:55be7cc4ef10 | 232 | lGrn=0; |
diavad | 0:55be7cc4ef10 | 233 | //BT.printf("Se han ingreso los parametros del PID\n"); |
diavad | 0:55be7cc4ef10 | 234 | } |
diavad | 0:55be7cc4ef10 | 235 | else if(strncmp(msg,"s",1)==0 || strncmp(msg,"S",1)==0){ |
diavad | 0:55be7cc4ef10 | 236 | PC.printf("Salida en Bajo\n"); |
diavad | 0:55be7cc4ef10 | 237 | lRed=0; |
diavad | 0:55be7cc4ef10 | 238 | lGrn=1; |
diavad | 0:55be7cc4ef10 | 239 | pwmOut.write(0.0f); |
diavad | 0:55be7cc4ef10 | 240 | AnOut.write(0.0f); |
diavad | 0:55be7cc4ef10 | 241 | |
diavad | 0:55be7cc4ef10 | 242 | //BT.printf("Se han ingreso los parametros del PID\n"); |
diavad | 0:55be7cc4ef10 | 243 | } |
diavad | 0:55be7cc4ef10 | 244 | else{ |
diavad | 0:55be7cc4ef10 | 245 | BT.printf("El parametro ingresado no es correcto\n"); |
diavad | 0:55be7cc4ef10 | 246 | } |
diavad | 0:55be7cc4ef10 | 247 | } |
diavad | 0:55be7cc4ef10 | 248 | } |