andres castaño / Mbed 2 deprecated PID

Programa Final

Dependencies:   mbed PID motoresDC

main.cpp@0:27f6b9086106, 2020-11-27 (annotated)

Committer:
20172573073
Date:
Fri Nov 27 23:41:33 2020 +0000
Revision:
0:27f6b9086106
Programa;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
20172573073 0:27f6b9086106 1 #include "mbed.h"
20172573073 0:27f6b9086106 2 #include "PID.h"
20172573073 0:27f6b9086106 3 #include "motoresDC.h"
20172573073 0:27f6b9086106 4 #include "rtos.h"
20172573073 0:27f6b9086106 5
20172573073 0:27f6b9086106 6 InterruptIn PIN_in(D2);
20172573073 0:27f6b9086106 7 DigitalOut r1(D3);
20172573073 0:27f6b9086106 8 DigitalOut r2(D4);
20172573073 0:27f6b9086106 9 MotoresDC carro(PTE31,D3, D4, D9, D12, D13);
20172573073 0:27f6b9086106 10 Timer t;
20172573073 0:27f6b9086106 11 Serial pc(USBTX, USBRX);
20172573073 0:27f6b9086106 12 Thread thread1;
20172573073 0:27f6b9086106 13 Thread thread2;
20172573073 0:27f6b9086106 14 Thread thread3;
20172573073 0:27f6b9086106 15 Thread thread4;
20172573073 0:27f6b9086106 16
20172573073 0:27f6b9086106 17 Semaphore one_slot(1);
20172573073 0:27f6b9086106 18 Mutex mutex1;
20172573073 0:27f6b9086106 19
20172573073 0:27f6b9086106 20 volatile uint8_t n;
20172573073 0:27f6b9086106 21 float timeold;
20172573073 0:27f6b9086106 22 unsigned int N;
20172573073 0:27f6b9086106 23 int x=0,z=0,q=0,dato=-1,dato1,dato2,dato3,dato4,dato5,dato6,dato7,dato8;
20172573073 0:27f6b9086106 24 float v,h,dkp,dki,dkd,dset,N1;
20172573073 0:27f6b9086106 25 float kp=2.0,ki=5.0,kd=1.0;
20172573073 0:27f6b9086106 26
20172573073 0:27f6b9086106 27 PID mypid(kp,ki,kd,0.01);
20172573073 0:27f6b9086106 28 void enco()
20172573073 0:27f6b9086106 29 {
20172573073 0:27f6b9086106 30 one_slot.release();
20172573073 0:27f6b9086106 31 one_slot.wait();
20172573073 0:27f6b9086106 32 n++;
20172573073 0:27f6b9086106 33 one_slot.release();
20172573073 0:27f6b9086106 34 }
20172573073 0:27f6b9086106 35 void interrupcion_serial(void)
20172573073 0:27f6b9086106 36 {
20172573073 0:27f6b9086106 37 one_slot.release();
20172573073 0:27f6b9086106 38 one_slot.wait();
20172573073 0:27f6b9086106 39 dato=pc.getc();
20172573073 0:27f6b9086106 40 if(z==8) {
20172573073 0:27f6b9086106 41 z=0;
20172573073 0:27f6b9086106 42 }
20172573073 0:27f6b9086106 43 one_slot.release();
20172573073 0:27f6b9086106 44
20172573073 0:27f6b9086106 45 }
20172573073 0:27f6b9086106 46 void motor()
20172573073 0:27f6b9086106 47 {
20172573073 0:27f6b9086106 48 while(1) {
20172573073 0:27f6b9086106 49 if(q==8) {
20172573073 0:27f6b9086106 50
20172573073 0:27f6b9086106 51 if (t.read_ms() - timeold >= 1000) {
20172573073 0:27f6b9086106 52
20172573073 0:27f6b9086106 53 N = ( n * 60 ) / (( t.read_ms() - timeold )/ 1000 * 20 );
20172573073 0:27f6b9086106 54 n = 0;
20172573073 0:27f6b9086106 55 timeold = t.read_ms();
20172573073 0:27f6b9086106 56 //v = N * 3.1416 * 24 * 60 / 1000000;
20172573073 0:27f6b9086106 57 N1=(float(N)*(3.3))/120.0;
20172573073 0:27f6b9086106 58
20172573073 0:27f6b9086106 59 //Thread::wait(1000);
20172573073 0:27f6b9086106 60
20172573073 0:27f6b9086106 61 }
20172573073 0:27f6b9086106 62
20172573073 0:27f6b9086106 63 }
20172573073 0:27f6b9086106 64 }
20172573073 0:27f6b9086106 65 }
20172573073 0:27f6b9086106 66 void piD (){
20172573073 0:27f6b9086106 67 while(1){
20172573073 0:27f6b9086106 68 one_slot.release();
20172573073 0:27f6b9086106 69 one_slot.wait();
20172573073 0:27f6b9086106 70 mypid.setSetPoint(dset);
20172573073 0:27f6b9086106 71 mypid.setTunings(dkp,dki,dkd);
20172573073 0:27f6b9086106 72 mypid.setProcessValue(float(N));
20172573073 0:27f6b9086106 73 h=mypid.compute();
20172573073 0:27f6b9086106 74 carro.motorIzq(h);
20172573073 0:27f6b9086106 75 one_slot.release();
20172573073 0:27f6b9086106 76 }
20172573073 0:27f6b9086106 77 }
20172573073 0:27f6b9086106 78 void serial()
20172573073 0:27f6b9086106 79 {
20172573073 0:27f6b9086106 80 while(1) {
20172573073 0:27f6b9086106 81 if(q==8) {
20172573073 0:27f6b9086106 82
20172573073 0:27f6b9086106 83 one_slot.release();
20172573073 0:27f6b9086106 84 one_slot.wait();
20172573073 0:27f6b9086106 85 //pc.printf("kp: %f ki: %f kd: %f set: %f ",dkp,dki,dkd,dset);
20172573073 0:27f6b9086106 86 //pc.printf("RPM: %d km/h: %f ",N,v);
20172573073 0:27f6b9086106 87 // pc.printf("RPM: %d N1: %f ",N,N1);
20172573073 0:27f6b9086106 88 //pc.printf("PWM: %f setPont: %f \n\r",h,dset);
20172573073 0:27f6b9086106 89 int N3=int(N*42);
20172573073 0:27f6b9086106 90 pc.printf("%d\n",N3);
20172573073 0:27f6b9086106 91 Thread::wait(300);
20172573073 0:27f6b9086106 92 one_slot.release();
20172573073 0:27f6b9086106 93
20172573073 0:27f6b9086106 94 }
20172573073 0:27f6b9086106 95 }
20172573073 0:27f6b9086106 96 }
20172573073 0:27f6b9086106 97 void conver()
20172573073 0:27f6b9086106 98 {
20172573073 0:27f6b9086106 99 switch(dato) {
20172573073 0:27f6b9086106 100 case '1':
20172573073 0:27f6b9086106 101 x=1;
20172573073 0:27f6b9086106 102 break;
20172573073 0:27f6b9086106 103 case '2':
20172573073 0:27f6b9086106 104 x=2;
20172573073 0:27f6b9086106 105 break;
20172573073 0:27f6b9086106 106 case '3':
20172573073 0:27f6b9086106 107 x=3;
20172573073 0:27f6b9086106 108 break;
20172573073 0:27f6b9086106 109 case '4':
20172573073 0:27f6b9086106 110
20172573073 0:27f6b9086106 111 x=4;
20172573073 0:27f6b9086106 112 break;
20172573073 0:27f6b9086106 113 case '5':
20172573073 0:27f6b9086106 114 x=5;
20172573073 0:27f6b9086106 115 break;
20172573073 0:27f6b9086106 116 case '6':
20172573073 0:27f6b9086106 117 x=6;
20172573073 0:27f6b9086106 118 break;
20172573073 0:27f6b9086106 119 case '7':
20172573073 0:27f6b9086106 120 x=7;
20172573073 0:27f6b9086106 121 break;
20172573073 0:27f6b9086106 122 case '8':
20172573073 0:27f6b9086106 123 x=8;
20172573073 0:27f6b9086106 124 break;
20172573073 0:27f6b9086106 125 case '9':
20172573073 0:27f6b9086106 126 x=9;
20172573073 0:27f6b9086106 127 break;
20172573073 0:27f6b9086106 128 case '0':
20172573073 0:27f6b9086106 129 x=0;
20172573073 0:27f6b9086106 130 break;
20172573073 0:27f6b9086106 131 }
20172573073 0:27f6b9086106 132
20172573073 0:27f6b9086106 133 }
20172573073 0:27f6b9086106 134 void inicial()
20172573073 0:27f6b9086106 135 {
20172573073 0:27f6b9086106 136 while(1) {
20172573073 0:27f6b9086106 137 if(z==0 && dato>0) {
20172573073 0:27f6b9086106 138 conver();
20172573073 0:27f6b9086106 139 dato1=x;
20172573073 0:27f6b9086106 140 dato=-1;
20172573073 0:27f6b9086106 141 // pc.printf("d1: %d \n\r",dato1);
20172573073 0:27f6b9086106 142 z=1;
20172573073 0:27f6b9086106 143 } else if (z==1 && dato>0) {
20172573073 0:27f6b9086106 144 conver();
20172573073 0:27f6b9086106 145 dato2=x;
20172573073 0:27f6b9086106 146 dato=-1;
20172573073 0:27f6b9086106 147 // pc.printf("d2: %d \n\r",dato2);
20172573073 0:27f6b9086106 148 z=2;
20172573073 0:27f6b9086106 149 dkp=float(dato1)+float(dato2*0.1);
20172573073 0:27f6b9086106 150 // pc.printf("dkp: %f \n\r",dkp);
20172573073 0:27f6b9086106 151 } else if (z==2 && dato>0) {
20172573073 0:27f6b9086106 152 conver();
20172573073 0:27f6b9086106 153 dato3=x;
20172573073 0:27f6b9086106 154 dato=-1;
20172573073 0:27f6b9086106 155 // pc.printf("d3: %d \n\r",dato3);
20172573073 0:27f6b9086106 156 z=3;
20172573073 0:27f6b9086106 157 } else if (z==3 && dato>0) {
20172573073 0:27f6b9086106 158 conver();
20172573073 0:27f6b9086106 159 dato4=x;
20172573073 0:27f6b9086106 160 dato=-1;
20172573073 0:27f6b9086106 161 // pc.printf("d4: %d \n\r",dato4);
20172573073 0:27f6b9086106 162 z=4;
20172573073 0:27f6b9086106 163 dki=float(dato3)+float(dato4*0.1);
20172573073 0:27f6b9086106 164 // pc.printf("dki: %f \n\r",dki);
20172573073 0:27f6b9086106 165
20172573073 0:27f6b9086106 166 } else if (z==4 && dato>0) {
20172573073 0:27f6b9086106 167 conver();
20172573073 0:27f6b9086106 168 dato5=x;
20172573073 0:27f6b9086106 169 dato=-1;
20172573073 0:27f6b9086106 170 // pc.printf("d5: %d \n\r",dato5);
20172573073 0:27f6b9086106 171 z=5;
20172573073 0:27f6b9086106 172 } else if (z==5 && dato>0) {
20172573073 0:27f6b9086106 173 conver();
20172573073 0:27f6b9086106 174 dato6=x;
20172573073 0:27f6b9086106 175 dato=-1;
20172573073 0:27f6b9086106 176 // pc.printf("d6: %d \n\r",dato6);
20172573073 0:27f6b9086106 177 z=6;
20172573073 0:27f6b9086106 178 dkd=float(dato5)+float(dato6*0.1);
20172573073 0:27f6b9086106 179 // pc.printf("dkd: %f \n\r",dkd);
20172573073 0:27f6b9086106 180
20172573073 0:27f6b9086106 181 } else if (z==6 && dato>0) {
20172573073 0:27f6b9086106 182 conver();
20172573073 0:27f6b9086106 183 dato7=x;
20172573073 0:27f6b9086106 184 dato=-1;
20172573073 0:27f6b9086106 185 // pc.printf("d7: %d \n\r",dato7);
20172573073 0:27f6b9086106 186 z=7;
20172573073 0:27f6b9086106 187 } else if (z==7 && dato>0) {
20172573073 0:27f6b9086106 188 conver();
20172573073 0:27f6b9086106 189 dato8=x;
20172573073 0:27f6b9086106 190 dato=-1;
20172573073 0:27f6b9086106 191 // pc.printf("d8: %d m: %d \n\r",dato8);
20172573073 0:27f6b9086106 192 z=8;
20172573073 0:27f6b9086106 193 q=8;
20172573073 0:27f6b9086106 194 dset=float(dato7*10)+float(dato8);
20172573073 0:27f6b9086106 195 // pc.printf("dset: %f \n\r",dset);
20172573073 0:27f6b9086106 196 mypid.reset();
20172573073 0:27f6b9086106 197
20172573073 0:27f6b9086106 198 }
20172573073 0:27f6b9086106 199
20172573073 0:27f6b9086106 200
20172573073 0:27f6b9086106 201 }
20172573073 0:27f6b9086106 202 }
20172573073 0:27f6b9086106 203 int main()
20172573073 0:27f6b9086106 204 {
20172573073 0:27f6b9086106 205 pc.baud(9600);
20172573073 0:27f6b9086106 206 PIN_in.mode(PullUp);
20172573073 0:27f6b9086106 207 PIN_in.fall(&enco);
20172573073 0:27f6b9086106 208 pc.attach(&interrupcion_serial);
20172573073 0:27f6b9086106 209 thread1.start(motor);
20172573073 0:27f6b9086106 210 thread2.start(serial);
20172573073 0:27f6b9086106 211 thread3.start(inicial);
20172573073 0:27f6b9086106 212 thread4.start(piD);
20172573073 0:27f6b9086106 213 mypid.setOutputLimits(0.0,1.0);
20172573073 0:27f6b9086106 214 mypid.setMode(1);
20172573073 0:27f6b9086106 215 carro.frenarIzq();
20172573073 0:27f6b9086106 216 carro.detenerIzq();
20172573073 0:27f6b9086106 217 r1=1;
20172573073 0:27f6b9086106 218 r2=0;
20172573073 0:27f6b9086106 219 t.start();
20172573073 0:27f6b9086106 220 thread4.join();
20172573073 0:27f6b9086106 221
20172573073 0:27f6b9086106 222
20172573073 0:27f6b9086106 223 }