Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
main.cpp@23:c97e206cf2a7, 2015-10-14 (annotated)
- Committer:
- Fernon
- Date:
- Wed Oct 14 14:55:21 2015 +0000
- Revision:
- 23:c97e206cf2a7
- Parent:
- 22:2e1713475f5f
- Child:
- 24:711c7c388e57
Lijkt te werken, Hij blijft binnen de range van zoals ik heb gezegd dat hij moet blijven. Buiten de range pakt de out of range het over. Moet nog verder getest worden op fouten en motor 2 moet ook nog toegevoegd worden. Servo motor werkt ook goed.
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 | 22:2e1713475f5f | 25 | const double margin = 0.3; |
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 | 22:2e1713475f5f | 73 | scope.set(2,emg_value); |
Fernon | 16:b0ec8e6a8ad4 | 74 | scope.send(); |
Fernon | 16:b0ec8e6a8ad4 | 75 | } |
Fernon | 22:2e1713475f5f | 76 | void EMGsample() |
Fernon | 22:2e1713475f5f | 77 | { |
Fernon | 22:2e1713475f5f | 78 | // Lees het EMG signaal uit |
Fernon | 22:2e1713475f5f | 79 | //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben |
Fernon | 22:2e1713475f5f | 80 | emg_value = PotMeter.read(); |
Fernon | 22:2e1713475f5f | 81 | } |
Fernon | 22:2e1713475f5f | 82 | |
Fernon | 22:2e1713475f5f | 83 | |
Fernon | 22:2e1713475f5f | 84 | |
Fernon | 0:5a5f417fa1b2 | 85 | int main() |
Fernon | 0:5a5f417fa1b2 | 86 | { |
Fernon | 11:a9a23042fc9e | 87 | upperlimit = n*twopi; |
Fernon | 2:f0e9ffc5df09 | 88 | pc.baud(115200); |
Fernon | 2:f0e9ffc5df09 | 89 | PowerMotor.write(0); |
Fernon | 16:b0ec8e6a8ad4 | 90 | Sender.attach(Send,0.5/Fs); |
yc238 | 15:f7e2b20f4dca | 91 | MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde |
Fernon | 23:c97e206cf2a7 | 92 | sampleEMG.attach(EMGsample,0.5/Fs); |
Fernon | 23:c97e206cf2a7 | 93 | PowerServo.period_ms(20); |
Fernon | 0:5a5f417fa1b2 | 94 | while (true) { |
Fernon | 17:c5eea26e171d | 95 | Encoder.reset(); |
Fernon | 11:a9a23042fc9e | 96 | if (Button == 0) { |
Fernon | 11:a9a23042fc9e | 97 | Excecute =! Excecute; |
Fernon | 17:c5eea26e171d | 98 | } |
Fernon | 22:2e1713475f5f | 99 | while (Excecute ) { //Dit wordt gebruikt voor motor 1 |
Fernon | 23:c97e206cf2a7 | 100 | //pc.printf("Rotatie = %f \n", Rotatie); |
Fernon | 23:c97e206cf2a7 | 101 | //pc.printf("Out of Range = %i \n", OutRange); |
Fernon | 23:c97e206cf2a7 | 102 | if (Rotatie >= upperlimit-margin) { |
Fernon | 22:2e1713475f5f | 103 | Goal = upperlimit - margin; |
Fernon | 23:c97e206cf2a7 | 104 | OutRange = true; |
Fernon | 22:2e1713475f5f | 105 | } |
Fernon | 23:c97e206cf2a7 | 106 | if (Rotatie <= downlimit+margin) { |
Fernon | 22:2e1713475f5f | 107 | Goal = 0 + margin; |
Fernon | 23:c97e206cf2a7 | 108 | OutRange = true; |
Fernon | 22:2e1713475f5f | 109 | } |
Fernon | 23:c97e206cf2a7 | 110 | if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan |
Fernon | 23:c97e206cf2a7 | 111 | OutRange = false; |
Fernon | 22:2e1713475f5f | 112 | // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden |
Fernon | 23:c97e206cf2a7 | 113 | if (emg_value >= T2 ) { |
Fernon | 22:2e1713475f5f | 114 | Direction = 1; |
Fernon | 22:2e1713475f5f | 115 | v = 1; |
Fernon | 22:2e1713475f5f | 116 | } |
Fernon | 22:2e1713475f5f | 117 | if (emg_value >= T1 && emg_value <= T2) { |
Fernon | 22:2e1713475f5f | 118 | Direction = 0; |
Fernon | 22:2e1713475f5f | 119 | v = 1; |
Fernon | 23:c97e206cf2a7 | 120 | } |
Fernon | 23:c97e206cf2a7 | 121 | if (emg_value <= T1){ |
Fernon | 23:c97e206cf2a7 | 122 | Direction = 0; |
Fernon | 23:c97e206cf2a7 | 123 | v = 0; |
Fernon | 23:c97e206cf2a7 | 124 | } |
Fernon | 23:c97e206cf2a7 | 125 | } |
Fernon | 23:c97e206cf2a7 | 126 | } |
Fernon | 23:c97e206cf2a7 | 127 | while (false ) { //Dit wordt gebruikt voor motor 2 MOET NOG OMGEZET WORDEN NAAR MOTOR 2!!!! |
Fernon | 23:c97e206cf2a7 | 128 | if (Rotatie >= upperlimit-margin) { |
Fernon | 23:c97e206cf2a7 | 129 | Goal = upperlimit - margin; |
Fernon | 23:c97e206cf2a7 | 130 | OutRange = true; |
Fernon | 23:c97e206cf2a7 | 131 | } |
Fernon | 23:c97e206cf2a7 | 132 | if (Rotatie <= downlimit+margin) { |
Fernon | 23:c97e206cf2a7 | 133 | Goal = 0 + margin; |
Fernon | 23:c97e206cf2a7 | 134 | OutRange = true; |
Fernon | 23:c97e206cf2a7 | 135 | } |
Fernon | 23:c97e206cf2a7 | 136 | if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan |
Fernon | 23:c97e206cf2a7 | 137 | OutRange = false; |
Fernon | 23:c97e206cf2a7 | 138 | // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden |
Fernon | 23:c97e206cf2a7 | 139 | if (emg_value >= T2 ) { |
Fernon | 23:c97e206cf2a7 | 140 | Direction = 0; |
Fernon | 23:c97e206cf2a7 | 141 | v = 1; |
Fernon | 23:c97e206cf2a7 | 142 | } |
Fernon | 23:c97e206cf2a7 | 143 | if (emg_value >= T1 && emg_value <= T2) { |
Fernon | 23:c97e206cf2a7 | 144 | Direction = 1; |
Fernon | 23:c97e206cf2a7 | 145 | v = 1; |
Fernon | 22:2e1713475f5f | 146 | } else { |
Fernon | 22:2e1713475f5f | 147 | Direction = 0; |
Fernon | 22:2e1713475f5f | 148 | v = 0; |
Fernon | 22:2e1713475f5f | 149 | } |
Fernon | 18:0f7f57228901 | 150 | } |
Fernon | 22:2e1713475f5f | 151 | } |
Fernon | 22:2e1713475f5f | 152 | |
Fernon | 23:c97e206cf2a7 | 153 | if (Button2 == 0) { |
Fernon | 23:c97e206cf2a7 | 154 | PowerServo.write(0.27); |
Fernon | 23:c97e206cf2a7 | 155 | wait (1); |
Fernon | 23:c97e206cf2a7 | 156 | PowerServo.write(0.04); |
Fernon | 23:c97e206cf2a7 | 157 | wait (1); |
Fernon | 23:c97e206cf2a7 | 158 | Fire=Fire+1; |
Fernon | 23:c97e206cf2a7 | 159 | } |
Fernon | 23:c97e206cf2a7 | 160 | if (Fire == 3) { |
Fernon | 23:c97e206cf2a7 | 161 | wait (1); |
Fernon | 23:c97e206cf2a7 | 162 | Excecute = false; |
Fernon | 23:c97e206cf2a7 | 163 | Home = true; |
Fernon | 23:c97e206cf2a7 | 164 | } |
Fernon | 23:c97e206cf2a7 | 165 | |
Fernon | 23:c97e206cf2a7 | 166 | |
Fernon | 22:2e1713475f5f | 167 | while (Home) { |
Fernon | 23:c97e206cf2a7 | 168 | OutRange = false; |
Fernon | 23:c97e206cf2a7 | 169 | Goal = margin; // Het doel waar hij naar toe moet |
Fernon | 21:d0156eadcbfe | 170 | if (fabs(Error)<=0.0015) { |
Fernon | 21:d0156eadcbfe | 171 | timer.start(); |
Fernon | 21:d0156eadcbfe | 172 | } else { |
Fernon | 21:d0156eadcbfe | 173 | timer.stop(); |
Fernon | 21:d0156eadcbfe | 174 | timer.reset(); |
Fernon | 16:b0ec8e6a8ad4 | 175 | } |
Fernon | 22:2e1713475f5f | 176 | if (timer.read() >= 3) { |
Fernon | 21:d0156eadcbfe | 177 | Excecute=false; |
Fernon | 22:2e1713475f5f | 178 | Home = false; |
Fernon | 18:0f7f57228901 | 179 | Errori = 0; |
Fernon | 18:0f7f57228901 | 180 | Errord = 0; |
Fernon | 11:a9a23042fc9e | 181 | } |
Fernon | 8:a2b725b502d8 | 182 | } |
Fernon | 0:5a5f417fa1b2 | 183 | } |
Fernon | 22:2e1713475f5f | 184 | } |