Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
main.cpp@22:2e1713475f5f, 2015-10-12 (annotated)
- Committer:
- Fernon
- Date:
- Mon Oct 12 18:23:05 2015 +0000
- Revision:
- 22:2e1713475f5f
- Parent:
- 21:d0156eadcbfe
- Child:
- 23:c97e206cf2a7
First try on our real programme, not tested yet
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 | 22:2e1713475f5f | 8 | PwmOut PowerServo(D7); |
Fernon | 22:2e1713475f5f | 9 | DigitalIn Button(PTC6); |
Fernon | 22:2e1713475f5f | 10 | DigitalIn Button2(PTC4); |
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 | 22:2e1713475f5f | 17 | Ticker EMG; |
Fernon | 22:2e1713475f5f | 18 | Timer timer; |
Fernon | 22:2e1713475f5f | 19 | HIDScope scope(3); |
Fernon | 0:5a5f417fa1b2 | 20 | |
yc238 | 15:f7e2b20f4dca | 21 | |
Fernon | 22:2e1713475f5f | 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 downlimit = 0.3; |
Fernon | 22:2e1713475f5f | 26 | const double margin = 0.3; |
Fernon | 22:2e1713475f5f | 27 | double Rotatie = 0; //aantal graden dat de motor is gedraaid |
Fernon | 22:2e1713475f5f | 28 | double Rotatieup = 0; //aantal graden dat de motor is gedraaid in een bereik van n*pi |
Fernon | 22:2e1713475f5f | 29 | double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan |
Fernon | 22:2e1713475f5f | 30 | double Error = 0; |
Fernon | 22:2e1713475f5f | 31 | double Errord = 0; |
Fernon | 22:2e1713475f5f | 32 | double Errori = 0; |
Fernon | 22:2e1713475f5f | 33 | double Errorp = 0; |
Fernon | 22:2e1713475f5f | 34 | const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides |
Fernon | 22:2e1713475f5f | 35 | const double Kd = 10; |
Fernon | 22:2e1713475f5f | 36 | const double Ki = 0.2; |
Fernon | 22:2e1713475f5f | 37 | double v = 0; //snelheid van de motor (0-0.1 |
Fernon | 22:2e1713475f5f | 38 | double upperlimit; //max aantal rotaties |
Fernon | 22:2e1713475f5f | 39 | bool Excecute = false; |
Fernon | 22:2e1713475f5f | 40 | bool Home = 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 | 22:2e1713475f5f | 50 | |
Fernon | 22:2e1713475f5f | 51 | Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 22:2e1713475f5f | 52 | Errord = (Error-Errorp)/Fs; |
Fernon | 22:2e1713475f5f | 53 | Errorp = Error; |
Fernon | 22:2e1713475f5f | 54 | if (fabs(Error) <= 0.5) { |
Fernon | 22:2e1713475f5f | 55 | Errori = Errori + Error*1/Fs; |
Fernon | 22:2e1713475f5f | 56 | } else { |
Fernon | 22:2e1713475f5f | 57 | Errori = 0; |
Fernon | 22:2e1713475f5f | 58 | } |
Fernon | 18:0f7f57228901 | 59 | if (Error>=0) { |
yc238 | 14:b9c925a8176a | 60 | Direction=1; |
yc238 | 14:b9c925a8176a | 61 | } else { |
yc238 | 14:b9c925a8176a | 62 | Direction=0; |
yc238 | 14:b9c925a8176a | 63 | } |
Fernon | 22:2e1713475f5f | 64 | v=Kp*Error + Kd*Errord + Ki*Errori; |
yc238 | 14:b9c925a8176a | 65 | PowerMotor.write(fabs(v)); |
Fernon | 2:f0e9ffc5df09 | 66 | } |
yc238 | 15:f7e2b20f4dca | 67 | void Send() |
yc238 | 15:f7e2b20f4dca | 68 | { |
Fernon | 22:2e1713475f5f | 69 | Pulses = Encoder.getPulses(); |
Fernon | 22:2e1713475f5f | 70 | Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien |
Fernon | 22:2e1713475f5f | 71 | Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit |
yc238 | 15:f7e2b20f4dca | 72 | scope.set(0,Goal); |
yc238 | 15:f7e2b20f4dca | 73 | scope.set(1,Rotatieup); |
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 | 22:2e1713475f5f | 86 | |
Fernon | 0:5a5f417fa1b2 | 87 | int main() |
Fernon | 0:5a5f417fa1b2 | 88 | { |
Fernon | 11:a9a23042fc9e | 89 | upperlimit = n*twopi; |
Fernon | 2:f0e9ffc5df09 | 90 | pc.baud(115200); |
Fernon | 2:f0e9ffc5df09 | 91 | PowerMotor.write(0); |
Fernon | 16:b0ec8e6a8ad4 | 92 | Sender.attach(Send,0.5/Fs); |
yc238 | 15:f7e2b20f4dca | 93 | MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde |
Fernon | 22:2e1713475f5f | 94 | EMG.attach(EMGsample,0.5/Fs); |
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 | 22:2e1713475f5f | 101 | if (Rotatie >= upperlimit) { |
Fernon | 22:2e1713475f5f | 102 | Goal = upperlimit - margin; |
Fernon | 22:2e1713475f5f | 103 | } |
Fernon | 22:2e1713475f5f | 104 | if (Rotatie <= downlimit) { |
Fernon | 22:2e1713475f5f | 105 | Goal = 0 + margin; |
Fernon | 22:2e1713475f5f | 106 | } |
Fernon | 22:2e1713475f5f | 107 | |
Fernon | 22:2e1713475f5f | 108 | else { |
Fernon | 22:2e1713475f5f | 109 | // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden |
Fernon | 22:2e1713475f5f | 110 | if (emg_value => T2 ) { |
Fernon | 22:2e1713475f5f | 111 | Direction = 1; |
Fernon | 22:2e1713475f5f | 112 | v = 1; |
Fernon | 22:2e1713475f5f | 113 | } |
Fernon | 22:2e1713475f5f | 114 | if (emg_value >= T1 && emg_value <= T2) { |
Fernon | 22:2e1713475f5f | 115 | Direction = 0; |
Fernon | 22:2e1713475f5f | 116 | v = 1; |
Fernon | 22:2e1713475f5f | 117 | } else { |
Fernon | 22:2e1713475f5f | 118 | Direction = 0; |
Fernon | 22:2e1713475f5f | 119 | v = 0; |
Fernon | 22:2e1713475f5f | 120 | } |
Fernon | 18:0f7f57228901 | 121 | } |
Fernon | 22:2e1713475f5f | 122 | while (Excecute) { // Dit is voor motor 2, dus naar boven en naar beneden |
Fernon | 22:2e1713475f5f | 123 | OutofRange = true if |
Fernon | 22:2e1713475f5f | 124 | if (Rotatie >= upperlimit) { |
Fernon | 22:2e1713475f5f | 125 | Goal = upperlimit - margin; |
Fernon | 22:2e1713475f5f | 126 | } |
Fernon | 22:2e1713475f5f | 127 | if (Rotatie <= downlimit) { |
Fernon | 22:2e1713475f5f | 128 | Goal = 0 + margin; |
Fernon | 22:2e1713475f5f | 129 | } |
Fernon | 22:2e1713475f5f | 130 | |
Fernon | 22:2e1713475f5f | 131 | else { |
Fernon | 22:2e1713475f5f | 132 | // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden |
Fernon | 22:2e1713475f5f | 133 | if (emg_value => T2 ) { |
Fernon | 22:2e1713475f5f | 134 | Direction = 1; |
Fernon | 22:2e1713475f5f | 135 | v = 1; |
Fernon | 22:2e1713475f5f | 136 | } |
Fernon | 22:2e1713475f5f | 137 | if (emg_value >= T1 && emg_value <= T2) { |
Fernon | 22:2e1713475f5f | 138 | Direction = 0; |
Fernon | 22:2e1713475f5f | 139 | v = 1; |
Fernon | 22:2e1713475f5f | 140 | } else { |
Fernon | 22:2e1713475f5f | 141 | Direction = 0; |
Fernon | 22:2e1713475f5f | 142 | v = 0; |
Fernon | 22:2e1713475f5f | 143 | } |
Fernon | 22:2e1713475f5f | 144 | } |
Fernon | 22:2e1713475f5f | 145 | } |
Fernon | 22:2e1713475f5f | 146 | if (Button2 = 0) { |
Fernon | 22:2e1713475f5f | 147 | // servo motor activation --> make a function of it |
Fernon | 22:2e1713475f5f | 148 | Fire=Fire+1; |
Fernon | 22:2e1713475f5f | 149 | } |
Fernon | 22:2e1713475f5f | 150 | if (Fire == 3) { |
Fernon | 22:2e1713475f5f | 151 | wait (1); |
Fernon | 22:2e1713475f5f | 152 | Excecute = false; |
Fernon | 22:2e1713475f5f | 153 | Home = true; |
Fernon | 22:2e1713475f5f | 154 | } |
Fernon | 22:2e1713475f5f | 155 | } |
Fernon | 22:2e1713475f5f | 156 | |
Fernon | 22:2e1713475f5f | 157 | while (Home) { |
Fernon | 22:2e1713475f5f | 158 | Goal = 0.3; // Het doel waar hij naar toe moet |
Fernon | 21:d0156eadcbfe | 159 | if (fabs(Error)<=0.0015) { |
Fernon | 21:d0156eadcbfe | 160 | timer.start(); |
Fernon | 21:d0156eadcbfe | 161 | } else { |
Fernon | 21:d0156eadcbfe | 162 | timer.stop(); |
Fernon | 21:d0156eadcbfe | 163 | timer.reset(); |
Fernon | 16:b0ec8e6a8ad4 | 164 | } |
Fernon | 22:2e1713475f5f | 165 | if (timer.read() >= 3) { |
Fernon | 21:d0156eadcbfe | 166 | Excecute=false; |
Fernon | 22:2e1713475f5f | 167 | Home = false; |
Fernon | 18:0f7f57228901 | 168 | Errori = 0; |
Fernon | 18:0f7f57228901 | 169 | Errord = 0; |
Fernon | 11:a9a23042fc9e | 170 | } |
Fernon | 8:a2b725b502d8 | 171 | } |
Fernon | 0:5a5f417fa1b2 | 172 | } |
Fernon | 22:2e1713475f5f | 173 | } |