Janine Klein Rot / Mbed 2 deprecated PIPO

Ping Pong robot

Dependencies:   mbed

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?

UserRevisionLine numberNew 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