![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Ping Pong robot
main.cpp@3:d36d3cc7ee23, 2014-10-14 (annotated)
- Committer:
- JKleinRot
- Date:
- Tue Oct 14 14:22:18 2014 +0000
- Revision:
- 3:d36d3cc7ee23
- Parent:
- 2:fbda25b453b3
14-10-14 versie met commentaar en tips van Gijs
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JKleinRot | 0:c37e4d7f1f56 | 1 | #include "mbed.h" //Mbed bibliotheek inladen, standaard functies |
JKleinRot | 0:c37e4d7f1f56 | 2 | #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC |
JKleinRot | 0:c37e4d7f1f56 | 3 | #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder |
JKleinRot | 0:c37e4d7f1f56 | 4 | |
JKleinRot | 2:fbda25b453b3 | 5 | #define SAMPLETIME_EMG 0.005 //Constanten definiëren en een waarde toekennen |
JKleinRot | 2:fbda25b453b3 | 6 | #define A1 2 |
JKleinRot | 2:fbda25b453b3 | 7 | #define A2 -157.9 |
JKleinRot | 2:fbda25b453b3 | 8 | #define B0 1 |
JKleinRot | 2:fbda25b453b3 | 9 | #define B1 -1.964 |
JKleinRot | 2:fbda25b453b3 | 10 | #define B2 0.9651 |
JKleinRot | 2:fbda25b453b3 | 11 | #define C1 1.158 |
JKleinRot | 2:fbda25b453b3 | 12 | #define C2 -0.4112 |
JKleinRot | 2:fbda25b453b3 | 13 | #define D0 1 |
JKleinRot | 2:fbda25b453b3 | 14 | #define D1 -1.709 |
JKleinRot | 2:fbda25b453b3 | 15 | #define D2 0.9625 |
JKleinRot | 3:d36d3cc7ee23 | 16 | #define E1 1.823 |
JKleinRot | 3:d36d3cc7ee23 | 17 | #define E2 -0.8372 |
JKleinRot | 3:d36d3cc7ee23 | 18 | #define F1 0.007438 |
JKleinRot | 3:d36d3cc7ee23 | 19 | #define F2 0.00701 |
JKleinRot | 0:c37e4d7f1f56 | 20 | |
JKleinRot | 1:7066a6113fbe | 21 | //Pinverdeling en naamgeving variabelen |
JKleinRot | 2:fbda25b453b3 | 22 | Encoder motor_arm1( //Encoder motor arm 1 |
JKleinRot | 2:fbda25b453b3 | 23 | Encoder motor_arm2( //Encoder motor arm 2 |
JKleinRot | 0:c37e4d7f1f56 | 24 | |
JKleinRot | 2:fbda25b453b3 | 25 | PwmOut pwm_motor_arm1( //PWM naar motor arm 1 |
JKleinRot | 2:fbda25b453b3 | 26 | PwmOut pwm_motor_arm2( //PWM naar motor arm 2 |
JKleinRot | 0:c37e4d7f1f56 | 27 | |
JKleinRot | 2:fbda25b453b3 | 28 | DigitalOut dir_motor_arm1( //Richting motor arm 1 |
JKleinRot | 2:fbda25b453b3 | 29 | DigitalOut dir_motor_arm2( //Richting motor arm 2 |
JKleinRot | 0:c37e4d7f1f56 | 30 | |
JKleinRot | 2:fbda25b453b3 | 31 | AnalogIn emg_bi( //EMG signalen biceps |
JKleinRot | 2:fbda25b453b3 | 32 | AnalogIn emg_tri( //EMG signalen triceps |
JKleinRot | 0:c37e4d7f1f56 | 33 | |
JKleinRot | 2:fbda25b453b3 | 34 | Ticker ticker_sample_EMG( //Uitlezen EMG iedere zoveel seconden |
JKleinRot | 0:c37e4d7f1f56 | 35 | |
JKleinRot | 2:fbda25b453b3 | 36 | |
JKleinRot | 2:fbda25b453b3 | 37 | //Gedefinieerde datatypes en variabelen |
JKleinRot | 0:c37e4d7f1f56 | 38 | bool rust = false |
JKleinRot | 0:c37e4d7f1f56 | 39 | bool kalibratie_positie = false |
JKleinRot | 0:c37e4d7f1f56 | 40 | bool kalibratie_EMG = false |
JKleinRot | 0:c37e4d7f1f56 | 41 | char lcd_r1 = new char[16]; //LCD regel 1, 16 karakters |
JKleinRot | 0:c37e4d7f1f56 | 42 | char lcd_r2 = new char[16]; //LCD regel 2, 16 karakters |
JKleinRot | 2:fbda25b453b3 | 43 | void sample_bi_EMG //Acties voor inladen EMG |
JKleinRot | 2:fbda25b453b3 | 44 | void sample_tri_EMG //Acties voor inladen EMG |
JKleinRot | 2:fbda25b453b3 | 45 | float xbk //x(n) biceps |
JKleinRot | 2:fbda25b453b3 | 46 | float xbk1 //x(n-1) biceps |
JKleinRot | 2:fbda25b453b3 | 47 | float xbk2 //x(n-2) biceps |
JKleinRot | 2:fbda25b453b3 | 48 | float ybk //y(n) biceps |
JKleinRot | 2:fbda25b453b3 | 49 | float ybk1 //y(n-1) biceps |
JKleinRot | 2:fbda25b453b3 | 50 | float ybk2 //y(n-2) biceps |
JKleinRot | 2:fbda25b453b3 | 51 | |
JKleinRot | 2:fbda25b453b3 | 52 | float xtk //x(n) triceps |
JKleinRot | 2:fbda25b453b3 | 53 | float xtk1 //x(n-1) triceps |
JKleinRot | 2:fbda25b453b3 | 54 | float xtk2 //x(n-2) triceps |
JKleinRot | 2:fbda25b453b3 | 55 | float ytk //y(n) triceps |
JKleinRot | 2:fbda25b453b3 | 56 | float ytk1 //y(n-1) triceps |
JKleinRot | 2:fbda25b453b3 | 57 | float ytk2 //y(n-2) triceps |
JKleinRot | 2:fbda25b453b3 | 58 | |
JKleinRot | 0:c37e4d7f1f56 | 59 | |
JKleinRot | 0:c37e4d7f1f56 | 60 | //Beginwaarden voor variabelen |
JKleinRot | 0:c37e4d7f1f56 | 61 | |
JKleinRot | 2:fbda25b453b3 | 62 | xbk1 = 0 //xbk(n-1) = 0 |
JKleinRot | 2:fbda25b453b3 | 63 | xbk2 = 0 //xbk(n-2) = 0 |
JKleinRot | 2:fbda25b453b3 | 64 | ybk1 = 0 //ybk(n-1) = 0 |
JKleinRot | 2:fbda25b453b3 | 65 | ybk2 = 0 //ybk(n-2) = 0 |
JKleinRot | 2:fbda25b453b3 | 66 | |
JKleinRot | 2:fbda25b453b3 | 67 | xtk1 = 0 //xtk(n-1) = 0 |
JKleinRot | 2:fbda25b453b3 | 68 | xtk2 = 0 //xtk(n-2) = 0 |
JKleinRot | 2:fbda25b453b3 | 69 | ytk1 = 0 //ytk(n-1) = 0 |
JKleinRot | 2:fbda25b453b3 | 70 | ytk2 = 0 //ytk(n-2) = 0 |
JKleinRot | 2:fbda25b453b3 | 71 | |
JKleinRot | 2:fbda25b453b3 | 72 | |
JKleinRot | 2:fbda25b453b3 | 73 | //PC baud rate |
JKleinRot | 2:fbda25b453b3 | 74 | pc.baud(38400) |
JKleinRot | 2:fbda25b453b3 | 75 | |
JKleinRot | 0:c37e4d7f1f56 | 76 | int main{ |
JKleinRot | 2:fbda25b453b3 | 77 | |
JKleinRot | 0:c37e4d7f1f56 | 78 | if (rust = false && kalibratie_positie = false && kalibratie_EMG = false){ |
JKleinRot | 1:7066a6113fbe | 79 | lcd_r1 = " BMT M9 GR. 4 "; //Tekst in LCD scherm |
JKleinRot | 1:7066a6113fbe | 80 | lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst in LCD scherm |
JKleinRot | 1:7066a6113fbe | 81 | wait(2); //2 seconden wachten |
JKleinRot | 0:c37e4d7f1f56 | 82 | rust = true |
JKleinRot | 2:fbda25b453b3 | 83 | } |
JKleinRot | 0:c37e4d7f1f56 | 84 | |
JKleinRot | 0:c37e4d7f1f56 | 85 | if (rust = true && kalibratie_positie = false && kalibratie_EMG = false){ |
JKleinRot | 1:7066a6113fbe | 86 | wait(1); //1 seconde wachten |
JKleinRot | 0:c37e4d7f1f56 | 87 | |
JKleinRot | 2:fbda25b453b3 | 88 | pwm_motor_arm1 = 0.02036 //Dutycycle motor arm 1/100 |
JKleinRot | 2:fbda25b453b3 | 89 | dir_motor_arm1 = // 1 of 0 //Draairichting motor arm 1 |
JKleinRot | 0:c37e4d7f1f56 | 90 | |
JKleinRot | 2:fbda25b453b3 | 91 | while(motor_arm1 < 363){ //Zolang aantal encoderpulsen < 363 --> PWM motor arm 1 blijft gelijk |
JKleinRot | 0:c37e4d7f1f56 | 92 | pwm_motor_arm1 = 0.02036 |
JKleinRot | 0:c37e4d7f1f56 | 93 | } |
JKleinRot | 0:c37e4d7f1f56 | 94 | |
JKleinRot | 2:fbda25b453b3 | 95 | pwm_motor_arm1 = 0 //Na while loop PWM motor arm 1 op nul |
JKleinRot | 1:7066a6113fbe | 96 | motor_arm1 = 0 //Encoderpulsen op nul zetten |
JKleinRot | 0:c37e4d7f1f56 | 97 | |
JKleinRot | 2:fbda25b453b3 | 98 | pwm_motor_arm2 = 0.02398 //Dutycycle motor arm 2/100 |
JKleinRot | 2:fbda25b453b3 | 99 | dir_motor_arm2 = // 1 of 0 //Draairichting motor arm 2 |
JKleinRot | 0:c37e4d7f1f56 | 100 | |
JKleinRot | 2:fbda25b453b3 | 101 | while(motor_arm_2 < 787){ //Zolang aantal encoderpulsen < 787 --> PWM motor arm 3 blijft gelijk |
JKleinRot | 0:c37e4d7f1f56 | 102 | pwm_motor_arm2 = 0.02398 |
JKleinRot | 0:c37e4d7f1f56 | 103 | } |
JKleinRot | 0:c37e4d7f1f56 | 104 | |
JKleinRot | 2:fbda25b453b3 | 105 | pwm_motor_arm2 = 0 //Na while loop PWM motor arm 2 op nul |
JKleinRot | 2:fbda25b453b3 | 106 | motor_arm2 = 0 //Encoderpulsen op nul zetten |
JKleinRot | 0:c37e4d7f1f56 | 107 | |
JKleinRot | 1:7066a6113fbe | 108 | lcd_r1 = " Beginpositie " //Tekst in LCD scherm |
JKleinRot | 1:7066a6113fbe | 109 | lcd_r2 = " ingesteld " //Tekst in LCD scherm |
JKleinRot | 1:7066a6113fbe | 110 | wait(2); //2 seconden wachten |
JKleinRot | 0:c37e4d7f1f56 | 111 | kalibratie_positie = true |
JKleinRot | 0:c37e4d7f1f56 | 112 | } |
JKleinRot | 0:c37e4d7f1f56 | 113 | |
JKleinRot | 0:c37e4d7f1f56 | 114 | if (rust = true && kalibratie_positie = true && kalibratie_EMG = false){ |
JKleinRot | 1:7066a6113fbe | 115 | wait(1); //1 seconde wachten |
JKleinRot | 2:fbda25b453b3 | 116 | |
JKleinRot | 2:fbda25b453b3 | 117 | //EMG inladen |
JKleinRot | 2:fbda25b453b3 | 118 | void sample_EMG{ |
JKleinRot | 2:fbda25b453b3 | 119 | xbk = emg_bi.read(); //Uitlezen EMG signaal biceps |
JKleinRot | 2:fbda25b453b3 | 120 | xtk = emg_tri.read(); //Uitlezen EMG signaal triceps |
JKleinRot | 3:d36d3cc7ee23 | 121 | filter(); |
JKleinRot | 3:d36d3cc7ee23 | 122 | } |
JKleinRot | 3:d36d3cc7ee23 | 123 | |
JKleinRot | 3:d36d3cc7ee23 | 124 | |
JKleinRot | 3:d36d3cc7ee23 | 125 | void filter() |
JKleinRot | 3:d36d3cc7ee23 | 126 | { |
JKleinRot | 3:d36d3cc7ee23 | 127 | /* do filtering */ |
JKleinRot | 3:d36d3cc7ee23 | 128 | |
JKleinRot | 3:d36d3cc7ee23 | 129 | } |
JKleinRot | 3:d36d3cc7ee23 | 130 | ticker_sample_EMG.attach(sample_EMG,SAMPLETIME_EMG) //Ticker gaat de void sample_EMG uitvoeren en dus de EMG uitlezen iedere 0.005 seconden |
JKleinRot | 3:d36d3cc7ee23 | 131 | |
JKleinRot | 3:d36d3cc7ee23 | 132 | while t<600{ |
JKleinRot | 3:d36d3cc7ee23 | 133 | xbkt(t)=xbk |
JKleinRot | 3:d36d3cc7ee23 | 134 | ybkt(t) = ybk |
JKleinRot | 3:d36d3cc7ee23 | 135 | xtkt(t) = xtk |
JKleinRot | 3:d36d3cc7ee23 | 136 | ytkt(t) = ytk |
JKleinRot | 3:d36d3cc7ee23 | 137 | t = t+1 |
JKleinRot | 2:fbda25b453b3 | 138 | } |
JKleinRot | 2:fbda25b453b3 | 139 | |
JKleinRot | 2:fbda25b453b3 | 140 | |
JKleinRot | 2:fbda25b453b3 | 141 | //Detrend |
JKleinRot | 3:d36d3cc7ee23 | 142 | n = 600; |
JKleinRot | 3:d36d3cc7ee23 | 143 | |
JKleinRot | 3:d36d3cc7ee23 | 144 | //hier wordt de som van de x en y waardes van het gemeten signaal berekend |
JKleinRot | 3:d36d3cc7ee23 | 145 | |
JKleinRot | 3:d36d3cc7ee23 | 146 | int i, n //n = het aantaal samples dat we hebben, is afhankelijk van het tijdinterval dat we kiezen |
JKleinRot | 3:d36d3cc7ee23 | 147 | for(i=0; i<n; i++) { |
JKleinRot | 3:d36d3cc7ee23 | 148 | sum_bk = sum_bk + ybk(i) //functie met line 88 voor een soomatie van alle waardes |
JKleinRot | 3:d36d3cc7ee23 | 149 | //sum = sum_bk |
JKleinRot | 3:d36d3cc7ee23 | 150 | sum_tk += ytk(i) //betekenis van de += dat de waardes altijd bij elkaar op worden geteld |
JKleinRot | 2:fbda25b453b3 | 151 | |
JKleinRot | 3:d36d3cc7ee23 | 152 | // ook mean van de x en dan met de juiste/betere manier |
JKleinRot | 3:d36d3cc7ee23 | 153 | } |
JKleinRot | 3:d36d3cc7ee23 | 154 | |
JKleinRot | 3:d36d3cc7ee23 | 155 | //hier wordt de mean van de x en y van het gemeten signaal gemeten |
JKleinRot | 3:d36d3cc7ee23 | 156 | |
JKleinRot | 3:d36d3cc7ee23 | 157 | mean_ybk = sum_bk./n |
JKleinRot | 3:d36d3cc7ee23 | 158 | mean_xbk = sum_xbk./n |
JKleinRot | 3:d36d3cc7ee23 | 159 | |
JKleinRot | 0:c37e4d7f1f56 | 160 | |
JKleinRot | 2:fbda25b453b3 | 161 | //Hoogdoorlaatfilter 2Hz |
JKleinRot | 2:fbda25b453b3 | 162 | while(true){ |
JKleinRot | 3:d36d3cc7ee23 | 163 | ybk = A1*ybk1+A2*ybk2+B0*xbk+B1*xbk1+B2*xbk2 //Overdracht 2e orde hoogdoorlaatfilter |
JKleinRot | 2:fbda25b453b3 | 164 | |
JKleinRot | 2:fbda25b453b3 | 165 | xbk2 = xbk1 //xbk(n-2) wordt xbk(n-1) in de volgende loop |
JKleinRot | 2:fbda25b453b3 | 166 | xbk1 = xbk //xbk(n-1) wordt xbk(n) in de volgende loop |
JKleinRot | 2:fbda25b453b3 | 167 | ybk2 = ybk1 //ybk(n-2) wordt ybk(n-1) in de volgende loop |
JKleinRot | 2:fbda25b453b3 | 168 | ybk1 = ybk //ybk(n-1) wordt ybk(n) in de volgende loop |
JKleinRot | 2:fbda25b453b3 | 169 | |
JKleinRot | 3:d36d3cc7ee23 | 170 | ytk = A1*ytk1+A2*ytk2+B0*xtk+B1*xtk1+B2*xtk2 //Overdracht 2e orde hoogdoorlaatfilter |
JKleinRot | 2:fbda25b453b3 | 171 | |
JKleinRot | 2:fbda25b453b3 | 172 | xtk2 = xtk1 //xtk(n-2) wordt xtk(n-1) in de volgende loop |
JKleinRot | 2:fbda25b453b3 | 173 | xtk1 = xtk //xtk(n-1) wordt xtk(n) in de volgende loop |
JKleinRot | 2:fbda25b453b3 | 174 | ytk2 = ytk1 //ytk(n-2) wordt ytk(n-1) in de volgende loop |
JKleinRot | 2:fbda25b453b3 | 175 | ytk1 = ytk //ytk(n-1) wordt ytk(n) in de volgende loop |
JKleinRot | 2:fbda25b453b3 | 176 | |
JKleinRot | 2:fbda25b453b3 | 177 | wait(SAMPLETIME_EMG); //0.005 seconden wachten |
JKleinRot | 2:fbda25b453b3 | 178 | } |
JKleinRot | 2:fbda25b453b3 | 179 | |
JKleinRot | 2:fbda25b453b3 | 180 | //Bandstop 40 - 60Hz |
JKleinRot | 2:fbda25b453b3 | 181 | while(true){ |
JKleinRot | 2:fbda25b453b3 | 182 | blabla = |
JKleinRot | 2:fbda25b453b3 | 183 | |
JKleinRot | 2:fbda25b453b3 | 184 | //Rectifier |
JKleinRot | 2:fbda25b453b3 | 185 | |
JKleinRot | 2:fbda25b453b3 | 186 | |
JKleinRot | 2:fbda25b453b3 | 187 | //Laagdoorlaatfilter 10Hz |
JKleinRot | 2:fbda25b453b3 | 188 | |
JKleinRot | 2:fbda25b453b3 | 189 | |
JKleinRot | 2:fbda25b453b3 | 190 | //Moving average 4e orde |
JKleinRot | 0:c37e4d7f1f56 | 191 | |
JKleinRot | 0:c37e4d7f1f56 | 192 | |
JKleinRot | 0:c37e4d7f1f56 | 193 | |
JKleinRot | 0:c37e4d7f1f56 | 194 | |
JKleinRot | 0:c37e4d7f1f56 | 195 |