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