Entrega numero 2 del curso procesadores 2019-2

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }