Con la variacion del encoder, nos moveremos a traves de los parametros del PID,luego al pulsarlo nos enviara una señal a un speaker.
Dependencies: Debounced QEI TextLCD mbed
main.cpp@0:cdf4bcffb35a, 2014-04-03 (annotated)
- Committer:
- Jesse
- Date:
- Thu Apr 03 20:19:31 2014 +0000
- Revision:
- 0:cdf4bcffb35a
Tarea4_Encoder
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jesse | 0:cdf4bcffb35a | 1 | //Jesse Mauricio Beltran |
Jesse | 0:cdf4bcffb35a | 2 | //Michelle Maceas Henao |
Jesse | 0:cdf4bcffb35a | 3 | |
Jesse | 0:cdf4bcffb35a | 4 | |
Jesse | 0:cdf4bcffb35a | 5 | #include "mbed.h" |
Jesse | 0:cdf4bcffb35a | 6 | #include "DebouncedIn.h" |
Jesse | 0:cdf4bcffb35a | 7 | #include "TextLCD.h" |
Jesse | 0:cdf4bcffb35a | 8 | #include "QEI.h" |
Jesse | 0:cdf4bcffb35a | 9 | |
Jesse | 0:cdf4bcffb35a | 10 | |
Jesse | 0:cdf4bcffb35a | 11 | QEI wheel (PTA16, PTA17, NC, 48); |
Jesse | 0:cdf4bcffb35a | 12 | TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 |
Jesse | 0:cdf4bcffb35a | 13 | |
Jesse | 0:cdf4bcffb35a | 14 | |
Jesse | 0:cdf4bcffb35a | 15 | DebouncedIn Enco(PTC5); |
Jesse | 0:cdf4bcffb35a | 16 | DebouncedIn button1(PTC10); |
Jesse | 0:cdf4bcffb35a | 17 | DebouncedIn button2(PTC6); |
Jesse | 0:cdf4bcffb35a | 18 | DebouncedIn button3(PTC11); |
Jesse | 0:cdf4bcffb35a | 19 | PwmOut Pwm(PTA5); |
Jesse | 0:cdf4bcffb35a | 20 | |
Jesse | 0:cdf4bcffb35a | 21 | |
Jesse | 0:cdf4bcffb35a | 22 | float pp=0.001; |
Jesse | 0:cdf4bcffb35a | 23 | float Dd=0.1; |
Jesse | 0:cdf4bcffb35a | 24 | int SP1=0,Sp=0,Kp1=0,Kp=0,Ki=0,Ki1=0,Kd=0,Kd1=0,v,m; |
Jesse | 0:cdf4bcffb35a | 25 | int C1=0x0E; // solo muestra el curzor |
Jesse | 0:cdf4bcffb35a | 26 | int i; // indice de la variable |
Jesse | 0:cdf4bcffb35a | 27 | |
Jesse | 0:cdf4bcffb35a | 28 | int PWMmodule(float pp,float Dd) |
Jesse | 0:cdf4bcffb35a | 29 | { |
Jesse | 0:cdf4bcffb35a | 30 | Pwm.period(pp); |
Jesse | 0:cdf4bcffb35a | 31 | Pwm.write(Dd); |
Jesse | 0:cdf4bcffb35a | 32 | wait(0.2); |
Jesse | 0:cdf4bcffb35a | 33 | Pwm.write(0); |
Jesse | 0:cdf4bcffb35a | 34 | return 0; |
Jesse | 0:cdf4bcffb35a | 35 | } |
Jesse | 0:cdf4bcffb35a | 36 | |
Jesse | 0:cdf4bcffb35a | 37 | int main() |
Jesse | 0:cdf4bcffb35a | 38 | |
Jesse | 0:cdf4bcffb35a | 39 | { |
Jesse | 0:cdf4bcffb35a | 40 | lcd.writeCommand(0x0E); |
Jesse | 0:cdf4bcffb35a | 41 | lcd.printf("Sp=0 Kp=0"); |
Jesse | 0:cdf4bcffb35a | 42 | lcd.locate(0,1); |
Jesse | 0:cdf4bcffb35a | 43 | lcd.printf("Ki=0 Kd=0"); |
Jesse | 0:cdf4bcffb35a | 44 | lcd.locate(2,0); |
Jesse | 0:cdf4bcffb35a | 45 | lcd.printf("="); |
Jesse | 0:cdf4bcffb35a | 46 | |
Jesse | 0:cdf4bcffb35a | 47 | while(1) |
Jesse | 0:cdf4bcffb35a | 48 | { |
Jesse | 0:cdf4bcffb35a | 49 | m=wheel.getPulses(); |
Jesse | 0:cdf4bcffb35a | 50 | |
Jesse | 0:cdf4bcffb35a | 51 | if(m!=v) { |
Jesse | 0:cdf4bcffb35a | 52 | |
Jesse | 0:cdf4bcffb35a | 53 | switch(i) { |
Jesse | 0:cdf4bcffb35a | 54 | |
Jesse | 0:cdf4bcffb35a | 55 | case 0: |
Jesse | 0:cdf4bcffb35a | 56 | Kd1=Kd; |
Jesse | 0:cdf4bcffb35a | 57 | |
Jesse | 0:cdf4bcffb35a | 58 | lcd.locate(2,0); |
Jesse | 0:cdf4bcffb35a | 59 | lcd.printf("= "); |
Jesse | 0:cdf4bcffb35a | 60 | lcd.locate(3,0); |
Jesse | 0:cdf4bcffb35a | 61 | lcd.printf("%d",Sp); |
Jesse | 0:cdf4bcffb35a | 62 | //Sp=m; |
Jesse | 0:cdf4bcffb35a | 63 | Sp=SP1+m; |
Jesse | 0:cdf4bcffb35a | 64 | if(Sp<0) |
Jesse | 0:cdf4bcffb35a | 65 | { |
Jesse | 0:cdf4bcffb35a | 66 | Sp=0; |
Jesse | 0:cdf4bcffb35a | 67 | wheel.reset(); |
Jesse | 0:cdf4bcffb35a | 68 | m=0; |
Jesse | 0:cdf4bcffb35a | 69 | } |
Jesse | 0:cdf4bcffb35a | 70 | break; |
Jesse | 0:cdf4bcffb35a | 71 | case 1: |
Jesse | 0:cdf4bcffb35a | 72 | SP1=Sp; |
Jesse | 0:cdf4bcffb35a | 73 | lcd.locate(10,0); |
Jesse | 0:cdf4bcffb35a | 74 | lcd.printf("= "); |
Jesse | 0:cdf4bcffb35a | 75 | lcd.locate(11,0); |
Jesse | 0:cdf4bcffb35a | 76 | lcd.printf("%d",Kp); |
Jesse | 0:cdf4bcffb35a | 77 | //Kp=m; |
Jesse | 0:cdf4bcffb35a | 78 | Kp=Kp1+m; |
Jesse | 0:cdf4bcffb35a | 79 | if(Kp<0) |
Jesse | 0:cdf4bcffb35a | 80 | { |
Jesse | 0:cdf4bcffb35a | 81 | Kp=0; |
Jesse | 0:cdf4bcffb35a | 82 | wheel.reset(); |
Jesse | 0:cdf4bcffb35a | 83 | m=0; |
Jesse | 0:cdf4bcffb35a | 84 | } |
Jesse | 0:cdf4bcffb35a | 85 | break; |
Jesse | 0:cdf4bcffb35a | 86 | case 2: |
Jesse | 0:cdf4bcffb35a | 87 | Kp1=Kp; |
Jesse | 0:cdf4bcffb35a | 88 | lcd.locate(2,1); |
Jesse | 0:cdf4bcffb35a | 89 | lcd.printf("= "); |
Jesse | 0:cdf4bcffb35a | 90 | lcd.locate(3,1); |
Jesse | 0:cdf4bcffb35a | 91 | lcd.printf("%d",Ki); |
Jesse | 0:cdf4bcffb35a | 92 | //Ki=m; |
Jesse | 0:cdf4bcffb35a | 93 | Ki=Ki1+m; |
Jesse | 0:cdf4bcffb35a | 94 | if(Ki<0) |
Jesse | 0:cdf4bcffb35a | 95 | { |
Jesse | 0:cdf4bcffb35a | 96 | Ki=0; |
Jesse | 0:cdf4bcffb35a | 97 | wheel.reset(); |
Jesse | 0:cdf4bcffb35a | 98 | m=0; |
Jesse | 0:cdf4bcffb35a | 99 | } |
Jesse | 0:cdf4bcffb35a | 100 | |
Jesse | 0:cdf4bcffb35a | 101 | break; |
Jesse | 0:cdf4bcffb35a | 102 | case 3: |
Jesse | 0:cdf4bcffb35a | 103 | Ki1=Ki; |
Jesse | 0:cdf4bcffb35a | 104 | lcd.locate(10,1); |
Jesse | 0:cdf4bcffb35a | 105 | lcd.printf("= "); |
Jesse | 0:cdf4bcffb35a | 106 | lcd.locate(11,1); |
Jesse | 0:cdf4bcffb35a | 107 | lcd.printf("%d",Kd); |
Jesse | 0:cdf4bcffb35a | 108 | Kd=Kd1+m; |
Jesse | 0:cdf4bcffb35a | 109 | if(Kd<0) |
Jesse | 0:cdf4bcffb35a | 110 | { |
Jesse | 0:cdf4bcffb35a | 111 | Kd=0; |
Jesse | 0:cdf4bcffb35a | 112 | wheel.reset(); |
Jesse | 0:cdf4bcffb35a | 113 | m=0; |
Jesse | 0:cdf4bcffb35a | 114 | } |
Jesse | 0:cdf4bcffb35a | 115 | |
Jesse | 0:cdf4bcffb35a | 116 | break; |
Jesse | 0:cdf4bcffb35a | 117 | } |
Jesse | 0:cdf4bcffb35a | 118 | v=m; |
Jesse | 0:cdf4bcffb35a | 119 | } |
Jesse | 0:cdf4bcffb35a | 120 | |
Jesse | 0:cdf4bcffb35a | 121 | |
Jesse | 0:cdf4bcffb35a | 122 | if(button3.falling() || Enco.falling()) |
Jesse | 0:cdf4bcffb35a | 123 | { |
Jesse | 0:cdf4bcffb35a | 124 | PWMmodule(pp,Dd); |
Jesse | 0:cdf4bcffb35a | 125 | wait(0.2); |
Jesse | 0:cdf4bcffb35a | 126 | wheel.reset(); |
Jesse | 0:cdf4bcffb35a | 127 | i++; |
Jesse | 0:cdf4bcffb35a | 128 | if(i>3) |
Jesse | 0:cdf4bcffb35a | 129 | { |
Jesse | 0:cdf4bcffb35a | 130 | i=0; |
Jesse | 0:cdf4bcffb35a | 131 | } |
Jesse | 0:cdf4bcffb35a | 132 | switch (i) |
Jesse | 0:cdf4bcffb35a | 133 | { |
Jesse | 0:cdf4bcffb35a | 134 | case 0: |
Jesse | 0:cdf4bcffb35a | 135 | lcd.locate(2,0); |
Jesse | 0:cdf4bcffb35a | 136 | lcd.printf("="); |
Jesse | 0:cdf4bcffb35a | 137 | break; |
Jesse | 0:cdf4bcffb35a | 138 | case 1: |
Jesse | 0:cdf4bcffb35a | 139 | lcd.locate(10,0); |
Jesse | 0:cdf4bcffb35a | 140 | lcd.printf("="); |
Jesse | 0:cdf4bcffb35a | 141 | break; |
Jesse | 0:cdf4bcffb35a | 142 | case 2: |
Jesse | 0:cdf4bcffb35a | 143 | lcd.locate(2,1); |
Jesse | 0:cdf4bcffb35a | 144 | lcd.printf("="); |
Jesse | 0:cdf4bcffb35a | 145 | break; |
Jesse | 0:cdf4bcffb35a | 146 | case 3: |
Jesse | 0:cdf4bcffb35a | 147 | lcd.locate(10,1); |
Jesse | 0:cdf4bcffb35a | 148 | lcd.printf("="); |
Jesse | 0:cdf4bcffb35a | 149 | break; |
Jesse | 0:cdf4bcffb35a | 150 | |
Jesse | 0:cdf4bcffb35a | 151 | |
Jesse | 0:cdf4bcffb35a | 152 | } |
Jesse | 0:cdf4bcffb35a | 153 | } |
Jesse | 0:cdf4bcffb35a | 154 | /*if(button3.falling()) |
Jesse | 0:cdf4bcffb35a | 155 | { |
Jesse | 0:cdf4bcffb35a | 156 | PWMmodule(pp,Dd); |
Jesse | 0:cdf4bcffb35a | 157 | } */ |
Jesse | 0:cdf4bcffb35a | 158 | } |
Jesse | 0:cdf4bcffb35a | 159 | } |