Alles in 1
Dependencies: Encoder HIDScope MODSERIAL QEI mbed
Fork of RoboBird3 by
main.cpp@24:711c7c388e57, 2015-10-14 (annotated)
- Committer:
- Fernon
- Date:
- Wed Oct 14 15:27:45 2015 +0000
- Revision:
- 24:711c7c388e57
- Parent:
- 23:c97e206cf2a7
- Child:
- 25:230bd4e1f3ef
Motor 1 werkt nu perfect (er moet nog wel even gekeken worden naar maximale rotatie, hier heb ik nog een fout in zitten). Motor 2 moet nog de range bepaald worden. Vuurknop werkt gewoon prima;
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 | 22:2e1713475f5f | 6 | DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down |
Fernon | 22:2e1713475f5f | 7 | PwmOut PowerMotor(D5); //van 0 tot 1 |
Fernon | 23:c97e206cf2a7 | 8 | PwmOut PowerServo(D3); |
Fernon | 22:2e1713475f5f | 9 | DigitalIn Button(PTC6); |
Fernon | 23:c97e206cf2a7 | 10 | DigitalIn Button2(PTA4); |
Fernon | 22:2e1713475f5f | 11 | AnalogIn PotMeter(A1); |
Fernon | 22:2e1713475f5f | 12 | AnalogIn EMG(A0); |
Fernon | 22:2e1713475f5f | 13 | QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder |
Fernon | 22:2e1713475f5f | 14 | Serial pc(USBTX, USBRX); |
Fernon | 22:2e1713475f5f | 15 | Ticker MotorWrite; |
Fernon | 22:2e1713475f5f | 16 | Ticker Sender; |
Fernon | 23:c97e206cf2a7 | 17 | Ticker sampleEMG; |
Fernon | 22:2e1713475f5f | 18 | Timer timer; |
Fernon | 22:2e1713475f5f | 19 | HIDScope scope(3); |
Fernon | 0:5a5f417fa1b2 | 20 | |
yc238 | 15:f7e2b20f4dca | 21 | |
Fernon | 23:c97e206cf2a7 | 22 | double emg_value; |
Fernon | 22:2e1713475f5f | 23 | const double twopi = 6.2831853071795; |
Fernon | 22:2e1713475f5f | 24 | int Pulses; |
Fernon | 24:711c7c388e57 | 25 | const double margin = 0.4; |
Fernon | 22:2e1713475f5f | 26 | double Rotatie = 0; //aantal graden dat de motor is gedraaid |
Fernon | 22:2e1713475f5f | 27 | double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan |
Fernon | 22:2e1713475f5f | 28 | double Error = 0; |
Fernon | 22:2e1713475f5f | 29 | double Errord = 0; |
Fernon | 22:2e1713475f5f | 30 | double Errori = 0; |
Fernon | 22:2e1713475f5f | 31 | double Errorp = 0; |
Fernon | 22:2e1713475f5f | 32 | const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides |
Fernon | 22:2e1713475f5f | 33 | const double Kd = 10; |
Fernon | 22:2e1713475f5f | 34 | const double Ki = 0.2; |
Fernon | 22:2e1713475f5f | 35 | double v = 0; //snelheid van de motor (0-0.1 |
Fernon | 22:2e1713475f5f | 36 | double upperlimit; //max aantal rotaties |
Fernon | 23:c97e206cf2a7 | 37 | const double downlimit = 0.3; |
Fernon | 22:2e1713475f5f | 38 | bool Excecute = false; |
Fernon | 22:2e1713475f5f | 39 | bool Home = false; |
Fernon | 23:c97e206cf2a7 | 40 | bool OutRange = false; |
Fernon | 22:2e1713475f5f | 41 | const double Fs=100; |
Fernon | 22:2e1713475f5f | 42 | const double T1 = 0.33333; // Treshold 1 |
Fernon | 22:2e1713475f5f | 43 | const double T2 = 0.66666; // Treshold 2 |
Fernon | 22:2e1713475f5f | 44 | int Fire = 0; |
Fernon | 10:e210675cbe71 | 45 | |
Fernon | 22:2e1713475f5f | 46 | double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien |
Fernon | 2:f0e9ffc5df09 | 47 | |
yc238 | 14:b9c925a8176a | 48 | void MotorSet() |
Fernon | 2:f0e9ffc5df09 | 49 | { |
Fernon | 23:c97e206cf2a7 | 50 | if (OutRange) { |
Fernon | 23:c97e206cf2a7 | 51 | Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 23:c97e206cf2a7 | 52 | Errord = (Error-Errorp)/Fs; |
Fernon | 23:c97e206cf2a7 | 53 | Errorp = Error; |
Fernon | 23:c97e206cf2a7 | 54 | if (fabs(Error) <= 0.5) { |
Fernon | 23:c97e206cf2a7 | 55 | Errori = Errori + Error*1/Fs; |
Fernon | 23:c97e206cf2a7 | 56 | } else { |
Fernon | 23:c97e206cf2a7 | 57 | Errori = 0; |
Fernon | 23:c97e206cf2a7 | 58 | } |
Fernon | 23:c97e206cf2a7 | 59 | if (Error>=0) { |
Fernon | 23:c97e206cf2a7 | 60 | Direction=1; |
Fernon | 23:c97e206cf2a7 | 61 | } else { |
Fernon | 23:c97e206cf2a7 | 62 | Direction=0; |
Fernon | 23:c97e206cf2a7 | 63 | } |
Fernon | 23:c97e206cf2a7 | 64 | v=Kp*Error + Kd*Errord + Ki*Errori; |
Fernon | 22:2e1713475f5f | 65 | } |
yc238 | 14:b9c925a8176a | 66 | PowerMotor.write(fabs(v)); |
Fernon | 2:f0e9ffc5df09 | 67 | } |
yc238 | 15:f7e2b20f4dca | 68 | void Send() |
yc238 | 15:f7e2b20f4dca | 69 | { |
Fernon | 22:2e1713475f5f | 70 | Pulses = Encoder.getPulses(); |
Fernon | 22:2e1713475f5f | 71 | Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien |
yc238 | 15:f7e2b20f4dca | 72 | scope.set(0,Goal); |
Fernon | 24:711c7c388e57 | 73 | scope.set(1,Rotatie); |
Fernon | 22:2e1713475f5f | 74 | scope.set(2,emg_value); |
Fernon | 16:b0ec8e6a8ad4 | 75 | scope.send(); |
Fernon | 16:b0ec8e6a8ad4 | 76 | } |
Fernon | 22:2e1713475f5f | 77 | void EMGsample() |
Fernon | 22:2e1713475f5f | 78 | { |
Fernon | 22:2e1713475f5f | 79 | // Lees het EMG signaal uit |
Fernon | 22:2e1713475f5f | 80 | //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben |
Fernon | 22:2e1713475f5f | 81 | emg_value = PotMeter.read(); |
Fernon | 22:2e1713475f5f | 82 | } |
Fernon | 22:2e1713475f5f | 83 | |
Fernon | 22:2e1713475f5f | 84 | |
Fernon | 22:2e1713475f5f | 85 | |
Fernon | 0:5a5f417fa1b2 | 86 | int main() |
Fernon | 0:5a5f417fa1b2 | 87 | { |
Fernon | 11:a9a23042fc9e | 88 | upperlimit = n*twopi; |
Fernon | 2:f0e9ffc5df09 | 89 | pc.baud(115200); |
Fernon | 2:f0e9ffc5df09 | 90 | PowerMotor.write(0); |
Fernon | 16:b0ec8e6a8ad4 | 91 | Sender.attach(Send,0.5/Fs); |
yc238 | 15:f7e2b20f4dca | 92 | MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde |
Fernon | 23:c97e206cf2a7 | 93 | sampleEMG.attach(EMGsample,0.5/Fs); |
Fernon | 23:c97e206cf2a7 | 94 | PowerServo.period_ms(20); |
Fernon | 0:5a5f417fa1b2 | 95 | while (true) { |
Fernon | 17:c5eea26e171d | 96 | Encoder.reset(); |
Fernon | 11:a9a23042fc9e | 97 | if (Button == 0) { |
Fernon | 11:a9a23042fc9e | 98 | Excecute =! Excecute; |
Fernon | 17:c5eea26e171d | 99 | } |
Fernon | 22:2e1713475f5f | 100 | while (Excecute ) { //Dit wordt gebruikt voor motor 1 |
Fernon | 23:c97e206cf2a7 | 101 | //pc.printf("Rotatie = %f \n", Rotatie); |
Fernon | 23:c97e206cf2a7 | 102 | //pc.printf("Out of Range = %i \n", OutRange); |
Fernon | 24:711c7c388e57 | 103 | if (Rotatie >= upperlimit) { |
Fernon | 22:2e1713475f5f | 104 | Goal = upperlimit - margin; |
Fernon | 23:c97e206cf2a7 | 105 | OutRange = true; |
Fernon | 22:2e1713475f5f | 106 | } |
Fernon | 24:711c7c388e57 | 107 | if (Rotatie <= downlimit) { |
Fernon | 22:2e1713475f5f | 108 | Goal = 0 + margin; |
Fernon | 23:c97e206cf2a7 | 109 | OutRange = true; |
Fernon | 22:2e1713475f5f | 110 | } |
Fernon | 24:711c7c388e57 | 111 | if (Rotatie >= margin && Rotatie <= upperlimit - margin) { |
Fernon | 23:c97e206cf2a7 | 112 | OutRange = false; |
Fernon | 24:711c7c388e57 | 113 | } |
Fernon | 24:711c7c388e57 | 114 | if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { |
Fernon | 22:2e1713475f5f | 115 | // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden |
Fernon | 23:c97e206cf2a7 | 116 | if (emg_value >= T2 ) { |
Fernon | 22:2e1713475f5f | 117 | Direction = 1; |
Fernon | 22:2e1713475f5f | 118 | v = 1; |
Fernon | 22:2e1713475f5f | 119 | } |
Fernon | 22:2e1713475f5f | 120 | if (emg_value >= T1 && emg_value <= T2) { |
Fernon | 22:2e1713475f5f | 121 | Direction = 0; |
Fernon | 22:2e1713475f5f | 122 | v = 1; |
Fernon | 24:711c7c388e57 | 123 | } |
Fernon | 24:711c7c388e57 | 124 | if (emg_value <= T1) { |
Fernon | 23:c97e206cf2a7 | 125 | Direction = 0; |
Fernon | 23:c97e206cf2a7 | 126 | v = 0; |
Fernon | 23:c97e206cf2a7 | 127 | } |
Fernon | 23:c97e206cf2a7 | 128 | } |
Fernon | 23:c97e206cf2a7 | 129 | } |
Fernon | 24:711c7c388e57 | 130 | |
Fernon | 23:c97e206cf2a7 | 131 | while (false ) { //Dit wordt gebruikt voor motor 2 MOET NOG OMGEZET WORDEN NAAR MOTOR 2!!!! |
Fernon | 23:c97e206cf2a7 | 132 | if (Rotatie >= upperlimit-margin) { |
Fernon | 23:c97e206cf2a7 | 133 | Goal = upperlimit - margin; |
Fernon | 23:c97e206cf2a7 | 134 | OutRange = true; |
Fernon | 23:c97e206cf2a7 | 135 | } |
Fernon | 23:c97e206cf2a7 | 136 | if (Rotatie <= downlimit+margin) { |
Fernon | 23:c97e206cf2a7 | 137 | Goal = 0 + margin; |
Fernon | 23:c97e206cf2a7 | 138 | OutRange = true; |
Fernon | 23:c97e206cf2a7 | 139 | } |
Fernon | 23:c97e206cf2a7 | 140 | if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan |
Fernon | 23:c97e206cf2a7 | 141 | OutRange = false; |
Fernon | 23:c97e206cf2a7 | 142 | // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden |
Fernon | 23:c97e206cf2a7 | 143 | if (emg_value >= T2 ) { |
Fernon | 23:c97e206cf2a7 | 144 | Direction = 0; |
Fernon | 23:c97e206cf2a7 | 145 | v = 1; |
Fernon | 23:c97e206cf2a7 | 146 | } |
Fernon | 23:c97e206cf2a7 | 147 | if (emg_value >= T1 && emg_value <= T2) { |
Fernon | 23:c97e206cf2a7 | 148 | Direction = 1; |
Fernon | 23:c97e206cf2a7 | 149 | v = 1; |
Fernon | 22:2e1713475f5f | 150 | } else { |
Fernon | 22:2e1713475f5f | 151 | Direction = 0; |
Fernon | 22:2e1713475f5f | 152 | v = 0; |
Fernon | 22:2e1713475f5f | 153 | } |
Fernon | 18:0f7f57228901 | 154 | } |
Fernon | 22:2e1713475f5f | 155 | } |
Fernon | 22:2e1713475f5f | 156 | |
Fernon | 23:c97e206cf2a7 | 157 | if (Button2 == 0) { |
Fernon | 23:c97e206cf2a7 | 158 | PowerServo.write(0.27); |
Fernon | 23:c97e206cf2a7 | 159 | wait (1); |
Fernon | 23:c97e206cf2a7 | 160 | PowerServo.write(0.04); |
Fernon | 23:c97e206cf2a7 | 161 | wait (1); |
Fernon | 23:c97e206cf2a7 | 162 | Fire=Fire+1; |
Fernon | 23:c97e206cf2a7 | 163 | } |
Fernon | 23:c97e206cf2a7 | 164 | if (Fire == 3) { |
Fernon | 23:c97e206cf2a7 | 165 | wait (1); |
Fernon | 23:c97e206cf2a7 | 166 | Excecute = false; |
Fernon | 23:c97e206cf2a7 | 167 | Home = true; |
Fernon | 23:c97e206cf2a7 | 168 | } |
Fernon | 23:c97e206cf2a7 | 169 | |
Fernon | 23:c97e206cf2a7 | 170 | |
Fernon | 22:2e1713475f5f | 171 | while (Home) { |
Fernon | 23:c97e206cf2a7 | 172 | OutRange = false; |
Fernon | 23:c97e206cf2a7 | 173 | Goal = margin; // Het doel waar hij naar toe moet |
Fernon | 21:d0156eadcbfe | 174 | if (fabs(Error)<=0.0015) { |
Fernon | 21:d0156eadcbfe | 175 | timer.start(); |
Fernon | 21:d0156eadcbfe | 176 | } else { |
Fernon | 21:d0156eadcbfe | 177 | timer.stop(); |
Fernon | 21:d0156eadcbfe | 178 | timer.reset(); |
Fernon | 16:b0ec8e6a8ad4 | 179 | } |
Fernon | 22:2e1713475f5f | 180 | if (timer.read() >= 3) { |
Fernon | 21:d0156eadcbfe | 181 | Excecute=false; |
Fernon | 22:2e1713475f5f | 182 | Home = false; |
Fernon | 18:0f7f57228901 | 183 | Errori = 0; |
Fernon | 18:0f7f57228901 | 184 | Errord = 0; |
Fernon | 11:a9a23042fc9e | 185 | } |
Fernon | 8:a2b725b502d8 | 186 | } |
Fernon | 0:5a5f417fa1b2 | 187 | } |
Fernon | 22:2e1713475f5f | 188 | } |