![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |