Marleen van Hoorn
inverse kinematics toegevoegd en tickers samengevoegd tot 1 ticker
Dependencies: HIDScope MODSERIAL biquadFilter mbed
Fork of
Project_script
by 
Marijke Zondag
Diff: main.cpp
- Revision:
- 23:97a976a8f0fc
- Parent:
- 22:5d956c93b3ae
- Child:
- 24:6d63ad38fef7
--- a/main.cpp Mon Oct 29 14:58:25 2018 +0000
+++ b/main.cpp Tue Oct 30 10:37:45 2018 +0000
@@ -17,24 +17,25 @@
InterruptIn button1 (D10); //Is this one available? We need to make a map of which pins are used for what.
InterruptIn button2 (D11);
-DigitalOut directionpin1 (D4); //Motor direction pin
-DigitalOut directionpin2 (D7);
+DigitalOut directionpin1 (D7);
+DigitalOut directionpin2 (D4);
+PwmOut pwmpin1 (D6);
+PwmOut pwmpin2 (D5);
DigitalOut ledr (LED_RED);
DigitalOut ledb (LED_BLUE);
DigitalOut ledg (LED_GREEN);
-PwmOut pwmpin1 (D5); //Pulse width modulation --> speed motor
-PwmOut pwmpin2 (D6);
-//MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
+MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
-HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
+//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
//Tickers
Ticker HIDScope_tick; //Ticker for HIDScope
Ticker filter_tick; //Ticker for EMG filter
Ticker MovAg_tick; //Ticker to calculate Moving Average
+Ticker Motor_tick; //Ticker motor aansturen
//Global variables
const float T = 0.002f; //Ticker period
@@ -110,6 +111,7 @@
if(x==4) //Reset back to x = -1
{
x = -1;
+ emg_cal=0; //reset, motors off
}
}
@@ -128,7 +130,7 @@
wait(0.001f); //Does there need to be a wait?
}
Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
- Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
+ Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
break; //Stop. Threshold is calculated, we will use this further in the code
}
case 1: //If calibration state 1:
@@ -141,7 +143,7 @@
wait(0.001f);
}
Mean1 = sum/sizeCal;
- Threshold1 = Mean1/2;
+ Threshold1 = Mean1/2;
break;
}
case 2: //If calibration state 2:
@@ -149,12 +151,12 @@
sum = 0.0;
for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
{
- StoreCal1[j] = emg2_filt;
+ StoreCal2[j] = emg2_filt;
sum+=StoreCal2[j];
wait(0.001f);
}
Mean2 = sum/sizeCal;
- Threshold2 = Mean2/2;
+ Threshold2 = Mean2/2;
break;
}
case 3: //EMG is calibrated, robot can be set to Home position.
@@ -173,17 +175,17 @@
void HIDScope_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_raw);
+ //scope.set(0,emg0_raw);
//scope.set(1,emg0_filt);
- scope.set(1,movAg0); //als moving average werkt
- scope.set(2,emg1_raw);
+ //scope.set(1,movAg0); //als moving average werkt
+ //scope.set(2,emg1_raw);
//scope.set(3,emg1_filt);
- scope.set(3,movAg1); //als moving average werkt
- scope.set(4,emg2_raw);
+ //scope.set(3,movAg1); //als moving average werkt
+ //scope.set(4,emg2_raw);
//scope.set(5,emg2_filt);
- scope.set(5,movAg2); //als moving average werkt
+ //scope.set(5,movAg2); //als moving average werkt
- scope.send(); //Send data to HIDScope server
+ //scope.send(); //Send data to HIDScope server
}
void EMGFilter0()
@@ -244,40 +246,18 @@
EMGFilter2();
}
-
-int main()
-{
- //pc.baud(115200);
- //pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
-
- emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain
- emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 );
- emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 );
-
- filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
- MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
- HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
-
- ledr = 1; //Begin led = blue, press button for first state of calibration --> led will turn red
- ledb = 0;
- ledg = 1;
-
- 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
-
- if(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
+void motor_control()
+{
+ while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
{
- //while (true)
- // {
+ // pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
+ // pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
+ // wait(2.0f);
if(movAg0>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);
+ directionpin1.write(1); //translatie vooruit
ledr = 1; //Blue
ledb = 0;
@@ -295,7 +275,7 @@
if(movAg1>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);
+ directionpin2.write(0); //rotatie omhoog
ledr = 0; //red
ledb = 1;
ledg = 1;
@@ -307,29 +287,58 @@
ledb = 0;
ledg = 0;
}
- if(movAg2>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);
+ // if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
+ //{
+ // pwmpin1 = 1;
+ //pwmpin2 = 1;
+ // directionpin1.write(0); //translatie achteruit
+ // directionpin2.write(1); //rotatie omlaag
+
+ //ledr = 1; //green
+ //ledb = 1;
+ //ledg = 0;
+ //}
+ //else //If not higher than the threshold, motors will not turn at all
+ //{
+ // pwmpin1 = 0;
+ // pwmpin2 = 0;
+
+ // ledr = 0; //white
+ // ledb = 0;
+ // ledg = 0;
+ //}
- ledr = 1; //green
- ledb = 1;
- ledg = 0;
- }
- else //If not higher than the threshold, motors will not turn at all
- {
- pwmpin1 = 0;
- pwmpin2 = 0;
-
- ledr = 0; //white
- ledb = 0;
- ledg = 0;
- }
-
-
+ break;
+ }
+}
+
+
+int main()
+{
+ pc.baud(115200);
+ pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
+ pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
+
+ emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain
+ emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 );
+ emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 );
- //}
- }
+ while(true)
+ {
+ filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
+ MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
+ Motor_tick.attach(&motor_control,T); //Test motor control
+
+ // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope 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);
+
+ pc.printf("x is %i\n\r",x);
+ pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
+ pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
+ //wait(2.0f);
+ }
}