![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Ping Pong robot
main.cpp
- Committer:
- JKleinRot
- Date:
- 2014-10-13
- Revision:
- 2:fbda25b453b3
- Parent:
- 1:7066a6113fbe
- Child:
- 3:d36d3cc7ee23
File content as of revision 2:fbda25b453b3:
#include "mbed.h" //Mbed bibliotheek inladen, standaard functies #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder #define SAMPLETIME_EMG 0.005 //Constanten definiëren en een waarde toekennen #define A1 2 #define A2 -157.9 #define B0 1 #define B1 -1.964 #define B2 0.9651 #define C1 1.158 #define C2 -0.4112 #define D0 1 #define D1 -1.709 #define D2 0.9625 //Pinverdeling en naamgeving variabelen Encoder motor_arm1( //Encoder motor arm 1 Encoder motor_arm2( //Encoder motor arm 2 PwmOut pwm_motor_arm1( //PWM naar motor arm 1 PwmOut pwm_motor_arm2( //PWM naar motor arm 2 DigitalOut dir_motor_arm1( //Richting motor arm 1 DigitalOut dir_motor_arm2( //Richting motor arm 2 AnalogIn emg_bi( //EMG signalen biceps AnalogIn emg_tri( //EMG signalen triceps Ticker ticker_sample_EMG( //Uitlezen EMG iedere zoveel seconden //Gedefinieerde datatypes en variabelen bool rust = false bool kalibratie_positie = false bool kalibratie_EMG = false char lcd_r1 = new char[16]; //LCD regel 1, 16 karakters char lcd_r2 = new char[16]; //LCD regel 2, 16 karakters void sample_bi_EMG //Acties voor inladen EMG void sample_tri_EMG //Acties voor inladen EMG float xbk //x(n) biceps float xbk1 //x(n-1) biceps float xbk2 //x(n-2) biceps float ybk //y(n) biceps float ybk1 //y(n-1) biceps float ybk2 //y(n-2) biceps float xtk //x(n) triceps float xtk1 //x(n-1) triceps float xtk2 //x(n-2) triceps float ytk //y(n) triceps float ytk1 //y(n-1) triceps float ytk2 //y(n-2) triceps //Beginwaarden voor variabelen xbk1 = 0 //xbk(n-1) = 0 xbk2 = 0 //xbk(n-2) = 0 ybk1 = 0 //ybk(n-1) = 0 ybk2 = 0 //ybk(n-2) = 0 xtk1 = 0 //xtk(n-1) = 0 xtk2 = 0 //xtk(n-2) = 0 ytk1 = 0 //ytk(n-1) = 0 ytk2 = 0 //ytk(n-2) = 0 //PC baud rate pc.baud(38400) int main{ if (rust = false && kalibratie_positie = false && kalibratie_EMG = false){ lcd_r1 = " BMT M9 GR. 4 "; //Tekst in LCD scherm lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst in LCD scherm wait(2); //2 seconden wachten rust = true } if (rust = true && kalibratie_positie = false && kalibratie_EMG = false){ wait(1); //1 seconde wachten pwm_motor_arm1 = 0.02036 //Dutycycle motor arm 1/100 dir_motor_arm1 = // 1 of 0 //Draairichting motor arm 1 while(motor_arm1 < 363){ //Zolang aantal encoderpulsen < 363 --> PWM motor arm 1 blijft gelijk pwm_motor_arm1 = 0.02036 } pwm_motor_arm1 = 0 //Na while loop PWM motor arm 1 op nul motor_arm1 = 0 //Encoderpulsen op nul zetten pwm_motor_arm2 = 0.02398 //Dutycycle motor arm 2/100 dir_motor_arm2 = // 1 of 0 //Draairichting motor arm 2 while(motor_arm_2 < 787){ //Zolang aantal encoderpulsen < 787 --> PWM motor arm 3 blijft gelijk pwm_motor_arm2 = 0.02398 } pwm_motor_arm2 = 0 //Na while loop PWM motor arm 2 op nul motor_arm2 = 0 //Encoderpulsen op nul zetten lcd_r1 = " Beginpositie " //Tekst in LCD scherm lcd_r2 = " ingesteld " //Tekst in LCD scherm wait(2); //2 seconden wachten kalibratie_positie = true } if (rust = true && kalibratie_positie = true && kalibratie_EMG = false){ wait(1); //1 seconde wachten //EMG inladen void sample_EMG{ xbk = emg_bi.read(); //Uitlezen EMG signaal biceps xtk = emg_tri.read(); //Uitlezen EMG signaal triceps } ticker_sample_EMG.attach(sample_EMG,SAMPLETIME_EMG) //Ticker gaat de void sample_EMG uitvoeren en dus de EMG uitlezen iedere 0.005 seconden //Detrend //Hoogdoorlaatfilter 2Hz while(true){ ybk = A1*ybk1-A2*ybk2+B0*xbk-B1*xbk1+B2*xbk2 //Overdracht 2e orde hoogdoorlaatfilter xbk2 = xbk1 //xbk(n-2) wordt xbk(n-1) in de volgende loop xbk1 = xbk //xbk(n-1) wordt xbk(n) in de volgende loop ybk2 = ybk1 //ybk(n-2) wordt ybk(n-1) in de volgende loop ybk1 = ybk //ybk(n-1) wordt ybk(n) in de volgende loop ytk = A1*ytk1-A2*ytk2+B0*xtk-B1*xtk1+B2*xtk2 //Overdracht 2e orde hoogdoorlaatfilter xtk2 = xtk1 //xtk(n-2) wordt xtk(n-1) in de volgende loop xtk1 = xtk //xtk(n-1) wordt xtk(n) in de volgende loop ytk2 = ytk1 //ytk(n-2) wordt ytk(n-1) in de volgende loop ytk1 = ytk //ytk(n-1) wordt ytk(n) in de volgende loop wait(SAMPLETIME_EMG); //0.005 seconden wachten } //Bandstop 40 - 60Hz while(true){ blabla = //Rectifier //Laagdoorlaatfilter 10Hz //Moving average 4e orde