Marijke Zondag
Make 2 motors turn
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of
Minor_test_serial
by 
Marijke Zondag
Revision 22:8e61050064a9, committed 2018-10-22
- Comitter:
- MarijkeZondag
- Date:
- Mon Oct 22 17:03:36 2018 +0000
- Parent:
- 21:1da43fdbd254
- Commit message:
- comments;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 1da43fdbd254 -r 8e61050064a9 main.cpp
--- a/main.cpp Mon Oct 22 13:44:40 2018 +0000
+++ b/main.cpp Mon Oct 22 17:03:36 2018 +0000
@@ -4,52 +4,44 @@
#include "HIDScope.h"
#include <math.h>
-AnalogIn emg0_in (A0);
-AnalogIn emg1_in (A1);
-AnalogIn emg2_in (A2);
+//ATTENTION: set mBed to version 151
+// set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
+// set MODSERIAL to version 44
+// set HIDScope to version 7
+// set biquadFilter to version 7
+
+AnalogIn emg0_in (A0); //First raw EMG signal input
+AnalogIn emg1_in (A1); //Second raw EMG signal input
+AnalogIn emg2_in (A2); //Third raw EMG signal input
InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
-InterruptIn button2 (D11);
-InterruptIn encoderA (D9);
-InterruptIn encoderB (D8);
+InterruptIn button2 (D11); //Buttons for switch calibration states
-DigitalOut directionpin1 (D4);
-DigitalOut directionpin2 (D7);
-DigitalOut ledr (LED_RED);
+DigitalOut ledr (LED_RED); //LEDs to show in which state you are in
DigitalOut ledb (LED_BLUE);
DigitalOut ledg (LED_GREEN);
-PwmOut pwmpin1 (D5);
-PwmOut pwmpin2 (D6);
+MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step
-
-MODSERIAL pc(USBTX, USBRX);
+HIDScope scope( 3 ); //HIDScope set to 3 channels for 3 muscles
//HIDscope
-Ticker sample_timer;
-HIDScope scope( 3 );
+Ticker sample_timer; //Ticker for HIDScope
+Ticker filter_tick; //Ticker for EMG filter
+Ticker MovAg_tick; //Ticker to calculate Moving Average
//Global variables
-int encoder = 0; //Starting point encoder
-const float T = 0.001f; //Ticker period
+int encoder = 0; //Starting point encoder (wordt nu nog niet gebruikt in de code)
+const float T = 0.001f; //Ticker period
//EMG filter
-double emgfilter0, emgfilter1, emgfilter2; //Filtered EMG data 0, 1 and 2
-const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated
+double emgfilter0, emgfilter1, emgfilter2; //Variables for filtered EMG data channel 0, 1 and 2
+const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number)
double sum, sum1, sum2, sum3; //variables used to sum elements in array
-double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
-double movAg0,movAg1,movAg2; //outcome of MovAg
+double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
+double movAg0,movAg1,movAg2; //outcome of MovAg (moet dit een array zijn??)
-//calibration
-int x = -1;
-int emg_cal = 0;
-const int sizeCal = 2000;
-double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];
-double Mean0,Mean1,Mean2;
-double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1;
-
-
-//Biquad
+//Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
BiQuadChain emg0band;
BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
@@ -65,27 +57,23 @@
BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
-BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
+BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
+//Functions
-//Tickers
-Ticker filter_tick;
-Ticker MovAg_tick;
-
-//Functions
+//HIDScope connection
void sample()
{
/* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
- scope.set(0, emg0_in.read() );
+ scope.set(0, emg0_in.read() ); //Raw EMG plot
scope.set(1, emg1_in.read() );
scope.set(2, emg2_in.read() );
- /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
- * Ensure that enough channels are available (HIDScope scope( 2 ))
- * Finally, send all channels to the PC at once */
- scope.send();
- /* To indicate that the function is working, the LED is toggled */
+
+ //scope.set(0, movAg0.read() ); Werkt niet!! Hoe dan wel moving average in HIDScope??
+
+ scope.send(); //Send data to HIDScope server
}
void EMGFilter0()
@@ -139,6 +127,7 @@
movAg0 = sum1/windowsize; //calculates an average in the array
movAg1 = sum2/windowsize;
movAg2 = sum3/windowsize;
+ //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
}
void emg_filtered() //Call all filter functions
@@ -149,227 +138,19 @@
MovAg();
}
-void switch_to_calibrate()
-{
- x++;
-
- if(x==0) //If x = 0, led is red
- {
- ledr = 0;
- ledb = 1;
- ledg = 1;
- }
- else if (x==1) //If x = 1, led is blue
- {
- ledr = 1;
- ledb = 0;
- ledg = 1;
- }
- else if (x == 2) //If x = 2, led is green
- {
- ledr = 1;
- ledb = 1;
- ledg = 0;
- }
- else //If x = 3, led is white
- {
- ledr = 0;
- ledb = 0;
- ledg = 0;
- }
-
- if(x==4) //Reset back to x = 0
- {
- x = -1;
- }
-}
-
-
-void calibrate(void)
-{
- switch(x)
- {
- case 0:
- {
- sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++)
- {
- StoreCal0[j] = emgfilter0;
- sum+=StoreCal0[j];
- wait(0.001f);
- }
- Mean0 = sum/sizeCal;
- Threshold0 = Mean0/2;
- break;
- }
- case 1:
- {
- sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++)
- {
- StoreCal1[j] = emgfilter1;
- sum+=StoreCal1[j];
- wait(0.001f);
- }
- Mean1 = sum/sizeCal;
- Threshold1 = Mean1/2;
- break;
- }
- case 2:
- {
- sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++)
- {
- StoreCal1[j] = emgfilter2;
- sum+=StoreCal2[j];
- wait(0.001f);
- }
- Mean2 = sum/sizeCal;
- Threshold2 = Mean2/2;
- break;
- }
- case 3: //EMG is calibrated, robot can be set to Home position.
- {
- emg_cal = 1;
- wait(0.001f);
- break;
- }
- default: //Ensures nothing happens if x is not 0,1 or 2.
- {
- break;
- }
- }
-}
-
-void encoderA_rise()
-{
- if(encoderB==false)
- {
- encoder++;
- }
- else
- {
- encoder--;
- }
-}
-
-void encoderA_fall()
-{
- if(encoderB==true)
- {
- encoder++;
- }
- else
- {
- encoder--;
- }
-}
-
-void encoderB_rise()
-{
- if(encoderA==true)
- {
- encoder++;
- }
- else
- {
- encoder--;
- }
-}
-
-void encoderB_fall()
-{
- if(encoderA==false)
- {
- encoder++;
- }
- else
- {
- encoder--;
- }
-}
-
-
// Main function start.
int main()
-{
- //pc.baud(115200);
- //pc.printf("hello\n\r");
-
- ledr = 0; //Begin led = red, first state of calibration
- ledb = 1;
- ledg = 1;
-
- sample_timer.attach(&sample, 0.002); //HIDscope
-
- filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
- button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
- wait(0.2f);
- button2.rise(calibrate); //calibrate threshold for 3 muscles
- wait(0.2f);
-
- pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
+{
+ pc.baud(115200);
+ pc.printf("hello\n\r");
+
+ while(true)
+ {
- encoderA.rise(&encoderA_rise);
- encoderA.fall(&encoderA_fall);
- encoderB.rise(&encoderB_rise);
- encoderB.fall(&encoderB_fall);
-
- if(emg_cal==1)
- {
- while (true)
- {
- //Motor aansturen en encoder uitlezen
- //float u1 = potmetervalue1;
- //float u2 = potmetervalue2;
-
- //float m1 = ((u1*2.0f)-1.0f);
- //float m2 = ((u2*2.0f)-1.0f);
-
- //pwmpin1 = fabs(m1*0.6f)+0.4f; //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.
-
- if(emgfilter0>Threshold0)
- {
- pwmpin1 = 1;
- directionpin1.write(1);
- }
- else
- {
- pwmpin1 = 0;
- }
-
- if(emgfilter1>Threshold1)
- {
- pwmpin2 = 1;
- directionpin2.write(1);
- }
- else
- {
- pwmpin2 = 0;
- }
- if(emgfilter2>Threshold2)
- {
- pwmpin1 = 1;
- pwmpin2 = 2;
- directionpin1.write(1);
- directionpin2.write(1);
- }
- else
- {
- pwmpin1 = 0;
- pwmpin2 = 0;
- }
-
- //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom.
- //wait(0.01f); //zodat de code niet oneindig doorgaat.
- //pwmpin2 = fabs(m2*0.6f)+0.4f;
- //directionpin2.write(m2>0);
-
- //float encoderDegrees = float(encoder)*(360.0/8400.0);
-
- //pc.printf("Encoder count: %f \n\r",encoderDegrees);
+ sample_timer.attach(&sample, 0.002);
+ filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
+ pc.printf("\n\r Moving average EMG 3 is: %d \n\r",movAg2);
- }
+ }
}
-}