Janine Klein Rot
Ping Pong robot
Revision 2:fbda25b453b3, committed 2014-10-13
- Comitter:
- JKleinRot
- Date:
- Mon Oct 13 13:30:59 2014 +0000
- Parent:
- 1:7066a6113fbe
- Child:
- 3:d36d3cc7ee23
- Commit message:
- 14-10-13 eerste poging filters, eerste poging ticker, comments compleet
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Oct 13 07:10:34 2014 +0000
+++ b/main.cpp Mon Oct 13 13:30:59 2014 +0000
@@ -2,70 +2,104 @@
#include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC
#include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder
-#define BLABLA waarde //Constanten definiëren en een waarde toekennen
+#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_arm2( //
+Encoder motor_arm1( //Encoder motor arm 1
+Encoder motor_arm2( //Encoder motor arm 2
-PwmOut pwm_motor_arm1
-PwmOut pwm_motor_arm2
+PwmOut pwm_motor_arm1( //PWM naar motor arm 1
+PwmOut pwm_motor_arm2( //PWM naar motor arm 2
-DigitalOut dir_motor_arm1
-DigitalOut dir_motor_arm2
+DigitalOut dir_motor_arm1( //Richting motor arm 1
+DigitalOut dir_motor_arm2( //Richting motor arm 2
-AnalogIn emg_bi
-AnalogIn emg_tri
+AnalogIn emg_bi( //EMG signalen biceps
+AnalogIn emg_tri( //EMG signalen triceps
-Ticker sample_EMG
+Ticker ticker_sample_EMG( //Uitlezen EMG iedere zoveel seconden
-//Floats, bools
+
+//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{
- //pc baudrate
- pc.baud(//opzoeken!!)
-
- //ticker maken die iedere zoveel seconden de EMG data inlaadt
- sample_EMG.attach(setsample_EMGflag,0.005);
-
- while (1){ //Oneindige while loop
+
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/100
- dir_motor_arm1 = // 1 of 0 //Richting motor
+ 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 motor1 blijft gelijk
+ 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 motor1 op nul
+ 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
- dir_motor_arm2 = // 1 of 0
+ 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){
+ while(motor_arm_2 < 787){ //Zolang aantal encoderpulsen < 787 --> PWM motor arm 3 blijft gelijk
pwm_motor_arm2 = 0.02398
}
- pwm_motor_arm2 = 0
- motor_arm2 = 0
+ 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
@@ -75,14 +109,48 @@
if (rust = true && kalibratie_positie = true && kalibratie_EMG = false){
wait(1); //1 seconde wachten
-
- ybk = emg_bi.read();
- ytk = emg_tri.read();
+
+ //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
+
- //detrend
-
-
- //hoogdoorlaatfilter
+ //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