Alles in 1
Dependencies: Encoder HIDScope MODSERIAL QEI mbed
Fork of RoboBird3 by
main.cpp@20:473735947e52, 2015-10-09 (annotated)
- Committer:
- Fernon
- Date:
- Fri Oct 09 10:20:15 2015 +0000
- Revision:
- 20:473735947e52
- Parent:
- 19:b8d959e02e5d
- Child:
- 21:d0156eadcbfe
PID controller werkend, nu toegespits op groep. Ik heb een motor 1 en motor 2 toegevoegd, en dat als hij boven een bepaalde waarde uit komt gaat hij weer terug naar veilige waarde. EMG moet nog goed geimplementeerd worden
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Fernon | 0:5a5f417fa1b2 | 1 | #include "mbed.h" |
Fernon | 16:b0ec8e6a8ad4 | 2 | #include "QEI.h" |
Fernon | 8:a2b725b502d8 | 3 | #include "math.h" |
yc238 | 12:dcf90cafb2a5 | 4 | #include "HIDScope.h" |
Fernon | 0:5a5f417fa1b2 | 5 | |
Fernon | 20:473735947e52 | 6 | DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down |
Fernon | 1:bb11e38dda43 | 7 | PwmOut PowerMotor(D5); //van 0 tot 1 |
yc238 | 15:f7e2b20f4dca | 8 | PwmOut PowerServo(D7); |
Fernon | 16:b0ec8e6a8ad4 | 9 | DigitalIn Button(PTC6); |
Fernon | 18:0f7f57228901 | 10 | DigitalIn Button2(PTC4); |
Fernon | 1:bb11e38dda43 | 11 | AnalogIn PotMeter(A1); |
Fernon | 16:b0ec8e6a8ad4 | 12 | QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder |
Fernon | 1:bb11e38dda43 | 13 | Serial pc(USBTX, USBRX); |
yc238 | 14:b9c925a8176a | 14 | Ticker MotorWrite; |
yc238 | 15:f7e2b20f4dca | 15 | Ticker Sender; |
Fernon | 20:473735947e52 | 16 | Ticker EMG; |
Fernon | 16:b0ec8e6a8ad4 | 17 | Timer timer; |
yc238 | 13:c60942a1da8e | 18 | HIDScope scope(3); |
Fernon | 0:5a5f417fa1b2 | 19 | |
yc238 | 15:f7e2b20f4dca | 20 | |
Fernon | 10:e210675cbe71 | 21 | double z=0; //initiele waarde potmeter |
Fernon | 8:a2b725b502d8 | 22 | const double twopi = 6.2831853071795; |
Fernon | 8:a2b725b502d8 | 23 | int Pulses; |
Fernon | 20:473735947e52 | 24 | const double downlimit = 0.3 |
Fernon | 20:473735947e52 | 25 | const double margin = 0.3 |
Fernon | 20:473735947e52 | 26 | double Rotatie; //aantal graden dat de motor is gedraaid |
yc238 | 14:b9c925a8176a | 27 | double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi |
Fernon | 11:a9a23042fc9e | 28 | double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan |
Fernon | 8:a2b725b502d8 | 29 | double Error = 0; |
Fernon | 18:0f7f57228901 | 30 | double Errord = 0; |
Fernon | 18:0f7f57228901 | 31 | double Errori = 0; |
Fernon | 18:0f7f57228901 | 32 | double Errorp = 0; |
Fernon | 18:0f7f57228901 | 33 | const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides |
Fernon | 18:0f7f57228901 | 34 | const double Kd = 10; |
Fernon | 19:b8d959e02e5d | 35 | const double Ki = 0.2; |
Fernon | 10:e210675cbe71 | 36 | double v = 0; //snelheid van de motor (0-0.1 |
Fernon | 10:e210675cbe71 | 37 | double upperlimit; //max aantal rotaties |
Fernon | 11:a9a23042fc9e | 38 | bool Excecute = false; |
Fernon | 17:c5eea26e171d | 39 | bool Excecute2 = false; |
Fernon | 18:0f7f57228901 | 40 | const double Fs=100; |
Fernon | 10:e210675cbe71 | 41 | |
Fernon | 20:473735947e52 | 42 | double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien |
Fernon | 2:f0e9ffc5df09 | 43 | |
yc238 | 14:b9c925a8176a | 44 | void MotorSet() |
Fernon | 2:f0e9ffc5df09 | 45 | { |
Fernon | 18:0f7f57228901 | 46 | v=Kp*Error + Kd*Errord + Ki*Errori; |
Fernon | 18:0f7f57228901 | 47 | if (Error>=0) { |
yc238 | 14:b9c925a8176a | 48 | Direction=1; |
yc238 | 14:b9c925a8176a | 49 | } else { |
yc238 | 14:b9c925a8176a | 50 | Direction=0; |
yc238 | 14:b9c925a8176a | 51 | } |
yc238 | 14:b9c925a8176a | 52 | PowerMotor.write(fabs(v)); |
Fernon | 2:f0e9ffc5df09 | 53 | } |
yc238 | 15:f7e2b20f4dca | 54 | void Send() |
yc238 | 15:f7e2b20f4dca | 55 | { |
yc238 | 15:f7e2b20f4dca | 56 | scope.set(0,Goal); |
yc238 | 15:f7e2b20f4dca | 57 | scope.set(1,Rotatieup); |
yc238 | 15:f7e2b20f4dca | 58 | scope.set(2,Error); |
Fernon | 16:b0ec8e6a8ad4 | 59 | scope.send(); |
Fernon | 16:b0ec8e6a8ad4 | 60 | } |
Fernon | 20:473735947e52 | 61 | void EMGsignal() |
Fernon | 20:473735947e52 | 62 | { |
Fernon | 20:473735947e52 | 63 | // Lees het EMG signaal uit |
Fernon | 20:473735947e52 | 64 | //if (Treshold => x) { |
Fernon | 20:473735947e52 | 65 | // v = 1 |
Fernon | 20:473735947e52 | 66 | // } else { |
Fernon | 20:473735947e52 | 67 | // v = 0 |
Fernon | 20:473735947e52 | 68 | // } |
Fernon | 20:473735947e52 | 69 | // PowerMotor.write(v) |
Fernon | 20:473735947e52 | 70 | } |
Fernon | 20:473735947e52 | 71 | |
Fernon | 20:473735947e52 | 72 | |
Fernon | 20:473735947e52 | 73 | |
Fernon | 20:473735947e52 | 74 | |
Fernon | 0:5a5f417fa1b2 | 75 | int main() |
Fernon | 0:5a5f417fa1b2 | 76 | { |
Fernon | 11:a9a23042fc9e | 77 | upperlimit = n*twopi; |
Fernon | 2:f0e9ffc5df09 | 78 | pc.baud(115200); |
Fernon | 2:f0e9ffc5df09 | 79 | PowerMotor.write(0); |
Fernon | 16:b0ec8e6a8ad4 | 80 | Sender.attach(Send,0.5/Fs); |
yc238 | 15:f7e2b20f4dca | 81 | MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde |
Fernon | 20:473735947e52 | 82 | EMG.attach(EMGsignal,1/Fs); |
Fernon | 0:5a5f417fa1b2 | 83 | while (true) { |
Fernon | 17:c5eea26e171d | 84 | Encoder.reset(); |
Fernon | 11:a9a23042fc9e | 85 | if (Button == 0) { |
Fernon | 11:a9a23042fc9e | 86 | Excecute =! Excecute; |
Fernon | 17:c5eea26e171d | 87 | } |
Fernon | 20:473735947e52 | 88 | while (Excecute ) { //Dit wordt gebruikt voor motor 1 |
Fernon | 16:b0ec8e6a8ad4 | 89 | Pulses = Encoder.getPulses(); |
Fernon | 20:473735947e52 | 90 | Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien |
Fernon | 20:473735947e52 | 91 | Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit |
yc238 | 12:dcf90cafb2a5 | 92 | pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); |
Fernon | 20:473735947e52 | 93 | if (Rotatie >= upperlimit) { |
Fernon | 20:473735947e52 | 94 | Goal = upperlimit - margin; |
Fernon | 20:473735947e52 | 95 | Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 20:473735947e52 | 96 | Errord = (Error-Errorp)/Fs; |
Fernon | 20:473735947e52 | 97 | Errorp = Error; |
Fernon | 20:473735947e52 | 98 | if (fabs(Error) <= 0.5) { |
Fernon | 20:473735947e52 | 99 | Errori = Errori + Error*1/Fs; |
Fernon | 20:473735947e52 | 100 | } else { |
Fernon | 20:473735947e52 | 101 | Errori = 0; |
Fernon | 20:473735947e52 | 102 | } |
Fernon | 20:473735947e52 | 103 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 18:0f7f57228901 | 104 | } |
Fernon | 20:473735947e52 | 105 | if (Rotatie <= downlimit) { |
Fernon | 20:473735947e52 | 106 | Goal = 0 + margin; |
Fernon | 20:473735947e52 | 107 | Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 20:473735947e52 | 108 | Errord = (Error-Errorp)/Fs; |
Fernon | 20:473735947e52 | 109 | Errorp = Error; |
Fernon | 20:473735947e52 | 110 | if (fabs(Error) <= 0.5) { |
Fernon | 20:473735947e52 | 111 | Errori = Errori + Error*1/Fs; |
Fernon | 20:473735947e52 | 112 | } else { |
Fernon | 20:473735947e52 | 113 | Errori = 0; |
Fernon | 20:473735947e52 | 114 | } |
Fernon | 20:473735947e52 | 115 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 16:b0ec8e6a8ad4 | 116 | } |
Fernon | 20:473735947e52 | 117 | |
Fernon | 20:473735947e52 | 118 | else { |
Fernon | 20:473735947e52 | 119 | // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden |
Fernon | 20:473735947e52 | 120 | Goal = z*upperlimit; |
Fernon | 20:473735947e52 | 121 | Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 20:473735947e52 | 122 | Errord = (Error-Errorp)/Fs; |
Fernon | 20:473735947e52 | 123 | Errorp = Error; |
Fernon | 20:473735947e52 | 124 | if (fabs(Error) <= 0.5) { |
Fernon | 20:473735947e52 | 125 | Errori = Errori + Error*1/Fs; |
Fernon | 20:473735947e52 | 126 | } else { |
Fernon | 20:473735947e52 | 127 | Errori = 0; |
Fernon | 20:473735947e52 | 128 | } |
Fernon | 20:473735947e52 | 129 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 17:c5eea26e171d | 130 | } |
Fernon | 17:c5eea26e171d | 131 | } |
Fernon | 20:473735947e52 | 132 | while (Excecute) { // Dit is voor motor 2, dus naar boven en naar beneden |
Fernon | 20:473735947e52 | 133 | Pulses = Encoder.getPulses(); |
Fernon | 20:473735947e52 | 134 | Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien |
Fernon | 20:473735947e52 | 135 | Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit |
Fernon | 20:473735947e52 | 136 | pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); |
Fernon | 20:473735947e52 | 137 | if (Rotatie >= upperlimit) { |
Fernon | 20:473735947e52 | 138 | Goal = upperlimit - margin; |
Fernon | 20:473735947e52 | 139 | Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 20:473735947e52 | 140 | Errord = (Error-Errorp)/Fs; |
Fernon | 20:473735947e52 | 141 | Errorp = Error; |
Fernon | 20:473735947e52 | 142 | if (fabs(Error) <= 0.5) { |
Fernon | 20:473735947e52 | 143 | Errori = Errori + Error*1/Fs; |
Fernon | 20:473735947e52 | 144 | } else { |
Fernon | 20:473735947e52 | 145 | Errori = 0; |
Fernon | 20:473735947e52 | 146 | } |
Fernon | 20:473735947e52 | 147 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 20:473735947e52 | 148 | } |
Fernon | 20:473735947e52 | 149 | if (Rotatie <= downlimit) { |
Fernon | 20:473735947e52 | 150 | Goal = 0 + margin; |
Fernon | 20:473735947e52 | 151 | Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 20:473735947e52 | 152 | Errord = (Error-Errorp)/Fs; |
Fernon | 20:473735947e52 | 153 | Errorp = Error; |
Fernon | 20:473735947e52 | 154 | if (fabs(Error) <= 0.5) { |
Fernon | 20:473735947e52 | 155 | Errori = Errori + Error*1/Fs; |
Fernon | 20:473735947e52 | 156 | } else { |
Fernon | 20:473735947e52 | 157 | Errori = 0; |
Fernon | 20:473735947e52 | 158 | } |
Fernon | 20:473735947e52 | 159 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 20:473735947e52 | 160 | } |
Fernon | 20:473735947e52 | 161 | |
Fernon | 20:473735947e52 | 162 | else { |
Fernon | 20:473735947e52 | 163 | // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden |
Fernon | 20:473735947e52 | 164 | Goal = z*upperlimit; |
Fernon | 20:473735947e52 | 165 | Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 20:473735947e52 | 166 | Errord = (Error-Errorp)/Fs; |
Fernon | 20:473735947e52 | 167 | Errorp = Error; |
Fernon | 20:473735947e52 | 168 | if (fabs(Error) <= 0.5) { |
Fernon | 20:473735947e52 | 169 | Errori = Errori + Error*1/Fs; |
Fernon | 20:473735947e52 | 170 | } else { |
Fernon | 20:473735947e52 | 171 | Errori = 0; |
Fernon | 20:473735947e52 | 172 | } |
Fernon | 20:473735947e52 | 173 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 20:473735947e52 | 174 | } |
Fernon | 20:473735947e52 | 175 | } |
Fernon | 20:473735947e52 | 176 | |
Fernon | 20:473735947e52 | 177 | |
Fernon | 20:473735947e52 | 178 | |
Fernon | 20:473735947e52 | 179 | // Dit gedeelte moet vervangen worden if fired then Excecut2 = true after waiting 1 second |
Fernon | 20:473735947e52 | 180 | // if (fabs(Error)<=0.0015) { |
Fernon | 20:473735947e52 | 181 | // timer.start(); |
Fernon | 20:473735947e52 | 182 | // } else { |
Fernon | 20:473735947e52 | 183 | // timer.stop(); |
Fernon | 20:473735947e52 | 184 | // timer.reset(); |
Fernon | 20:473735947e52 | 185 | // } |
Fernon | 20:473735947e52 | 186 | // if (timer.read() >= 5) { |
Fernon | 20:473735947e52 | 187 | // Excecute=false; |
Fernon | 20:473735947e52 | 188 | // Excecute2 = true; |
Fernon | 20:473735947e52 | 189 | // Errori = 0; |
Fernon | 20:473735947e52 | 190 | // Errord = 0; |
Fernon | 20:473735947e52 | 191 | // } |
Fernon | 20:473735947e52 | 192 | |
Fernon | 20:473735947e52 | 193 | // } |
Fernon | 17:c5eea26e171d | 194 | while (Excecute2) { |
Fernon | 17:c5eea26e171d | 195 | Pulses = Encoder.getPulses(); |
Fernon | 17:c5eea26e171d | 196 | Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen |
Fernon | 17:c5eea26e171d | 197 | Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit |
Fernon | 17:c5eea26e171d | 198 | pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); |
Fernon | 20:473735947e52 | 199 | Goal = 0.3; // Het doel waar hij naar toe moet |
Fernon | 17:c5eea26e171d | 200 | Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 19:b8d959e02e5d | 201 | Errord = (Error-Errorp)/Fs; |
Fernon | 19:b8d959e02e5d | 202 | Errorp = Error; |
Fernon | 19:b8d959e02e5d | 203 | if (fabs(Error) <= 0.5) { |
Fernon | 19:b8d959e02e5d | 204 | Errori = Errori + Error*1/Fs; |
Fernon | 19:b8d959e02e5d | 205 | } else { |
Fernon | 19:b8d959e02e5d | 206 | Errori = 0; |
Fernon | 19:b8d959e02e5d | 207 | } |
Fernon | 17:c5eea26e171d | 208 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 18:0f7f57228901 | 209 | if (fabs(Error)<=0.0015) { |
Fernon | 17:c5eea26e171d | 210 | timer.start(); |
Fernon | 17:c5eea26e171d | 211 | } else { |
Fernon | 17:c5eea26e171d | 212 | timer.stop(); |
Fernon | 17:c5eea26e171d | 213 | timer.reset(); |
Fernon | 17:c5eea26e171d | 214 | } |
Fernon | 17:c5eea26e171d | 215 | if (timer.read() >= 5) { |
Fernon | 19:b8d959e02e5d | 216 | Excecute=false; |
Fernon | 19:b8d959e02e5d | 217 | Excecute2 = false; |
Fernon | 18:0f7f57228901 | 218 | Errori = 0; |
Fernon | 18:0f7f57228901 | 219 | Errord = 0; |
Fernon | 11:a9a23042fc9e | 220 | } |
Fernon | 8:a2b725b502d8 | 221 | } |
Fernon | 0:5a5f417fa1b2 | 222 | } |
Fernon | 17:c5eea26e171d | 223 | } |