Marijke Zondag
/
Project_script_calibration
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL biquadFilter mbed
Fork of
Milestone_sample
by 
Marijke Zondag
Revision 22:91ad5984b2f2, committed 2018-10-24
- Comitter:
- MarijkeZondag
- Date:
- Wed Oct 24 19:41:41 2018 +0000
- Parent:
- 21:1da43fdbd254
- Commit message:
- Kalibratie project versie Marijke AF
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 1da43fdbd254 -r 91ad5984b2f2 main.cpp
--- a/main.cpp Mon Oct 22 13:44:40 2018 +0000
+++ b/main.cpp Wed Oct 24 19:41:41 2018 +0000
@@ -1,158 +1,51 @@
#include "mbed.h"
#include "MODSERIAL.h"
-#include "BiQuad.h"
-#include "HIDScope.h"
#include <math.h>
AnalogIn emg0_in (A0);
AnalogIn emg1_in (A1);
AnalogIn emg2_in (A2);
-InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
+InterruptIn button1 (D10); //Is this one available? We need to make a map of which pins are used for what.
InterruptIn button2 (D11);
-InterruptIn encoderA (D9);
-InterruptIn encoderB (D8);
-DigitalOut directionpin1 (D4);
+DigitalOut directionpin1 (D4); //Motor direction pin
DigitalOut directionpin2 (D7);
+
DigitalOut ledr (LED_RED);
DigitalOut ledb (LED_BLUE);
DigitalOut ledg (LED_GREEN);
-PwmOut pwmpin1 (D5);
+PwmOut pwmpin1 (D5); //Pulse width modulation --> speed motor
PwmOut pwmpin2 (D6);
+MODSERIAL pc(USBTX, USBRX); //Serial communication to test if the code works
+
-MODSERIAL pc(USBTX, USBRX);
-
-//HIDscope
-Ticker sample_timer;
-HIDScope scope( 3 );
-
-//Global variables
-int encoder = 0; //Starting point encoder
-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
+//EMG filter variables (also necessary for calibration parts)
+double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
+double emg0_raw, emg1_raw, emg2_raw;
+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
-
-//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;
+double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
+double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
-
-//Biquad
-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 );
-BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
-
-BiQuadChain emg1band;
-BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
-BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
-BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
+//Calibration variables
+int x = -1; //Start switch, colour LED is blue.
+int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!)
+const int sizeCal = 2000; //size of the dataset used for calibration
+double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in
+double Mean0,Mean1,Mean2; //average of maximum tightening
+double Threshold0, Threshold1, Threshold2; //Threshold variables
-BiQuadChain emg2band;
-BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
-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 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
-
-
-//Tickers
-Ticker filter_tick;
-Ticker MovAg_tick;
//Functions
-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(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 */
-}
-
-void EMGFilter0()
-{
- double emg0 = emg0_in.read();
- double bandpass0 = emg0band.step(emg0);
- double absolute0 = fabs(bandpass0);
- double emgfilter0 = notch1.step(absolute0);
-}
-
-void EMGFilter1()
-{
- double emg1 = emg1_in.read();
- double bandpass1 = emg1band.step(emg1);
- double absolute1 = fabs(bandpass1);
- double emgfilter1 = notch2.step(absolute1);
-}
-
-void EMGFilter2()
-{
- double emg2 = emg2_in.read();
- double bandpass2 = emg2band.step(emg2);
- double absolute2 = fabs(bandpass2);
- double emgfilter2 = notch3.step(absolute2);
-}
-
-void MovAg() //Calculate moving average (MovAg)
-{
- for (int i = windowsize-1; i>=0; i--) //Make array of the last datapoints of the filtered signal
- {
- StoreArray0[i] = StoreArray0[i-1];
- StoreArray1[i] = StoreArray1[i-1];
- StoreArray2[i] = StoreArray2[i-1];
- }
-
- StoreArray0[0] = emgfilter0; //Stores the latest datapoint in the first element of the array
- StoreArray1[0] = emgfilter1;
- StoreArray2[0] = emgfilter2;
-
- sum1 = 0.0;
- sum2 = 0.0;
- sum3 = 0.0;
-
- for(int a = 0; a<= windowsize-1; a++) //Sum the elements in the array
- {
- sum1 += StoreArray0[a];
- sum2 += StoreArray1[a];
- sum3 += StoreArray2[a];
- }
-
- movAg0 = sum1/windowsize; //calculates an average in the array
- movAg1 = sum2/windowsize;
- movAg2 = sum3/windowsize;
-}
-
-void emg_filtered() //Call all filter functions
-{
- EMGFilter0();
- EMGFilter1();
- EMGFilter2();
- MovAg();
-}
void switch_to_calibrate()
{
- x++;
-
+ x++; //Every time function gets called, x increases. Every button press --> new calibration state.
+ //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1.
+
if(x==0) //If x = 0, led is red
{
ledr = 0;
@@ -165,20 +58,20 @@
ledb = 0;
ledg = 1;
}
- else if (x == 2) //If x = 2, led is green
+ else if (x==2) //If x = 2, led is green
{
ledr = 1;
ledb = 1;
ledg = 0;
}
- else //If x = 3, led is white
+ else //If x = 3 or 4, led is white
{
ledr = 0;
ledb = 0;
ledg = 0;
}
- if(x==4) //Reset back to x = 0
+ if(x==4) //Reset back to x = -1
{
x = -1;
}
@@ -189,25 +82,25 @@
{
switch(x)
{
- case 0:
+ case 0: //If calibration state 0:
{
sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++)
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
{
- StoreCal0[j] = emgfilter0;
+ StoreCal0[j] = emg0_filt;
sum+=StoreCal0[j];
- wait(0.001f);
+ wait(0.001f); //Does there need to be a wait?
}
- Mean0 = sum/sizeCal;
- Threshold0 = Mean0/2;
- break;
+ Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
+ Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
+ break; //Stop. Threshold is calculated, we will use this further in the code
}
- case 1:
+ case 1: //If calibration state 1:
{
- sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++)
+ sum = 0.0;
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
{
- StoreCal1[j] = emgfilter1;
+ StoreCal1[j] = emg1_filt;
sum+=StoreCal1[j];
wait(0.001f);
}
@@ -215,12 +108,12 @@
Threshold1 = Mean1/2;
break;
}
- case 2:
+ case 2: //If calibration state 2:
{
sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++)
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
{
- StoreCal1[j] = emgfilter2;
+ StoreCal1[j] = emg2_filt;
sum+=StoreCal2[j];
wait(0.001f);
}
@@ -228,148 +121,81 @@
Threshold2 = Mean2/2;
break;
}
- case 3: //EMG is calibrated, robot can be set to Home position.
+ case 3: //EMG is calibrated, robot can be set to Home position.
{
- emg_cal = 1;
+ emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
wait(0.001f);
break;
}
- default: //Ensures nothing happens if x is not 0,1 or 2.
+ 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");
+ pc.baud(115200);
+ pc.printf("hello\n\r"); //Check does code work
- ledr = 0; //Begin led = red, first state of calibration
- ledb = 1;
+ ledr = 1; //Begin led = blue, press button for first state of calibration --> led will turn red
+ ledb = 0;
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
+ button2.rise(calibrate); //Calibrate threshold for 3 muscles
wait(0.2f);
pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
-
- encoderA.rise(&encoderA_rise);
- encoderA.fall(&encoderA_fall);
- encoderB.rise(&encoderB_rise);
- encoderB.fall(&encoderB_fall);
- if(emg_cal==1)
+ if(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
{
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)
+ {
+
+ if(emg0_filt>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
{
pwmpin1 = 1;
directionpin1.write(1);
}
- else
+ else //If it is not higher than the threshold, the motor will not turn at all.
{
pwmpin1 = 0;
}
- if(emgfilter1>Threshold1)
+ if(emg1_filt>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor2 will turn in 1 direction
{
pwmpin2 = 1;
directionpin2.write(1);
}
- else
+ else //If not higher than the threshold, motor will not turn at all
{
pwmpin2 = 0;
}
- if(emgfilter2>Threshold2)
+ if(emg2_filt>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
{
pwmpin1 = 1;
pwmpin2 = 2;
directionpin1.write(1);
directionpin2.write(1);
}
- else
+ else //If not higher than the threshold, motors will not turn at all
{
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);
+
- }
+ }
+ }
+
+ //Opmerkingen Marijke: ik ben niet zeker van waar ik de while loop moet plaatsen.
+ //Misschien is aan het einde buiten de if beter, schrijf dan while(1){}.
+
+ //De manier van aansturen van de motoren is nu puur om te testen of de kalibratie werkt. Op deze manier kunnen
+ //We namelijk niet met 2 spieren 2 motoren tegelijk aansturen.
}
-}