Daniqe Kottelenberg
change at hidscope
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
emg_import
by 
Daniqe Kottelenberg
Diff: main.cpp
- Revision:
- 45:d0e9f586cd03
- Parent:
- 44:969348be74a5
- Child:
- 46:f3c205dfb749
--- a/main.cpp Tue Nov 01 15:40:43 2016 +0000
+++ b/main.cpp Tue Nov 01 16:24:09 2016 +0000
@@ -3,88 +3,111 @@
#include "HIDScope.h"
#include "BiQuad.h"
#include "MODSERIAL.h"
+#include "QEI.h"
//Define objects
-AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG biceps (r) in to c++
-AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++
-AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++
+ //EMG
+ AnalogIn emg_biceps_right_in( A0); //analog in to get EMG biceps (r) in to c++
+ AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++
+ AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++
-DigitalIn button_calibration_biceps (SW3); //button to start calibration biceps
-DigitalIn button_calibration_triceps (SW2); // button to start calibration tricps
+ //Encoder
+ DigitalIn encoder1A(D13);
+ DigitalIn encoder1B(D12);
+ DigitalIn encoder2A(D11);
+ DigitalIn encoder2B(D10);
+
+ //callibration buttons
+ DigitalIn button_calibration_biceps (SW3); //button to start calibration biceps
+ DigitalIn button_calibration_triceps (SW2); // button to start calibration tricps
-Ticker sample_timer; //ticker
-Ticker switch_function; //ticker
-Ticker ticker_calibration_biceps;
-Ticker ticker_calibration_triceps;
-HIDScope scope(5); //open 3 channels in hidscope
+ //tickers
+ Ticker sample_timer; //ticker
+ Ticker switch_function; //ticker
+ Ticker ticker_calibration_biceps;
+ Ticker ticker_calibration_triceps;
+
-MODSERIAL pc(USBTX, USBRX); //pc connection
-DigitalOut red(LED_RED);
-DigitalOut green(LED_GREEN);
-DigitalOut blue(LED_BLUE);
+//everything for monitoring
+ HIDScope scope(5); //open 5 channels in hidscope
+ MODSERIAL pc(USBTX, USBRX); //pc connection
+ DigitalOut red(LED_RED);
+ DigitalOut green(LED_GREEN);
+ DigitalOut blue(LED_BLUE);
+
//motors
-DigitalOut richting_motor1(D4);
-PwmOut pwm_motor1(D5);
-DigitalOut richting_motor2(D7);
-PwmOut pwm_motor2(D6);
+ DigitalOut richting_motor1(D4);
+ PwmOut pwm_motor1(D5);
+ DigitalOut richting_motor2(D7);
+ PwmOut pwm_motor2(D6);
//define variables
-//other
-int onoffsignal_biceps=0; // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
-int switch_signal_triceps=0; // switching between motors.
-volatile double cut_off_value_biceps_right =0.04; //gespecificeerd door floortje
-volatile double cut_off_value_biceps_left =-0.04;
-volatile double cut_off_value_triceps=-0.04; //gespecificeerd door floortje
-double signal_biceps_sum;
-double bicepstriceps_rightarm;
-int motorswitch= 0; //start van de teller wordt op nul gesteld
-//variables and constants for calibration
-const float percentage_max_triceps=0.3;
-const float percentage_max_biceps =0.3;
-double max_biceps; //calibration maximum biceps
-double max_triceps; //calibration maximum triceps
-
-//biceps arm 1, right arm
-double emg_biceps_right;
-double emg_filtered_high_biceps_right;
-double emg_abs_biceps_right;
-double emg_filtered_biceps_right;
-double emg_filtered_high_notch_1_biceps_right;
-//double emg_filtered_high_notch_1_2_biceps_right;
-
-//triceps arm 1, right arm
-double emg_triceps_right;
-double emg_filtered_high_triceps_right;
-double emg_abs_triceps_right;
-double emg_filtered_triceps_right;
-double emg_filtered_high_notch_1_triceps_right;
-
-//biceps arm 1, left arm
-double emg_biceps_left;
-double emg_filtered_high_biceps_left;
-double emg_abs_biceps_left;
-double emg_filtered_biceps_left;
-double emg_filtered_high_notch_1_biceps_left;
-
-//before abs filtering
-
-//b1 = biceps right arm
-BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
-BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
-
-//t1= triceps right arm
-BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
-BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
-
-//b2= biceps left arm
-BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
-BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
-
-//after abs filtering
-BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
-BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
-BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
+ //for motorcontrol
+ const int cw = 0; // motor should turn clockwise
+ const int ccw =1; // motor should turn counterclockwise
+ const float gearboxratio=131.25; // gearboxratio from encoder to motor
+ const float rev_rond=64.0; // revolutions per round of encoder
+ int onoffsignal_biceps=0; // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
+ int switch_signal_triceps=0; // switching between motors.
+
+ volatile double cut_off_value_biceps_right = 0.04; //tested, normal values. Can be changed by calibration
+ volatile double cut_off_value_biceps_left = -0.04; //volatiles becaused changen in interrupt
+ volatile double cut_off_value_triceps=-0.03;
+ double signal_biceps_sum;
+ double bicepstriceps_rightarm;
+ int motorswitch=0;
+ volatile double rev_counts_motor1=0;
+ volatile double rev_counts_motor2=0;
+ volatile double counts_encoder1;
+ volatile double counts_encoder2;
+
+ //variables and constants for calibration
+ const float percentage_max_triceps=0.3;
+ const float percentage_max_biceps =0.3;
+ double max_biceps; //calibration maximum biceps
+ double max_triceps; //calibration maximum triceps
+
+ //biceps arm 1, right arm
+ double emg_biceps_right;
+ double emg_filtered_high_biceps_right;
+ double emg_abs_biceps_right;
+ double emg_filtered_biceps_right;
+ double emg_filtered_high_notch_1_biceps_right;
+ //double emg_filtered_high_notch_1_2_biceps_right;
+
+ //triceps arm 1, right arm
+ double emg_triceps_right;
+ double emg_filtered_high_triceps_right;
+ double emg_abs_triceps_right;
+ double emg_filtered_triceps_right;
+ double emg_filtered_high_notch_1_triceps_right;
+
+ //biceps arm 1, left arm
+ double emg_biceps_left;
+ double emg_filtered_high_biceps_left;
+ double emg_abs_biceps_left;
+ double emg_filtered_biceps_left;
+ double emg_filtered_high_notch_1_biceps_left;
+
+ //before abs filtering
+
+ //b1 = biceps right arm
+ BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
+ BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
+
+ //t1= triceps right arm
+ BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
+ BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
+
+ //b2= biceps left arm
+ BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
+ BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
+
+ //after abs filtering
+ BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
+ BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
+ BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
//function teller
void SwitchN() { // maakt simpele functie die 1 bij n optelt
@@ -256,6 +279,8 @@
int main()
{
pc.baud(115200); //connect with pc with baudrate 115200
+QEI Encoder2(D12,D13, NC, rev_rond,QEI::X4_ENCODING); // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen.
+QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
sample_timer.attach(&filter, 0.001); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
switch_function.attach(&SwitchN,1.0); //switch is every second available
@@ -285,22 +310,33 @@
while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt
-
+ //motor control with muscles.
if (onoffsignal_biceps==-1) // als s ingedrukt wordt gebeurd het volgende
{
if (motorswitch%2==0) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
{
- richting_motor1 = 1;
+ richting_motor1 = ccw;
pwm_motor1 = 0.5;
pc.printf("ccw m1\r\n");
+ //encoder aan
+ counts_encoder1 = Encoder1.getPulses();
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+ counts_encoder2 = Encoder2.getPulses();
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
}
else // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
{
- richting_motor2 = 1;
+ richting_motor2 = ccw;
pwm_motor2 = 1;
+
+ //encoder aan
+ counts_encoder1 = Encoder1.getPulses();
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+ counts_encoder2 = Encoder2.getPulses();
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
}
@@ -309,25 +345,40 @@
{
if (motorswitch%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende
{
- richting_motor1 = 0;
+ richting_motor1 = cw;
pwm_motor1 = 0.5;
pc.printf("cw 1 aan\r\n");
+ //encoder aan
+ counts_encoder1 = Encoder1.getPulses();
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+ counts_encoder2 = Encoder2.getPulses();
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
}
else // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurt het onderstaande
{
- richting_motor2 = 0;
+ richting_motor2 = cw;
pwm_motor2 = 1;
-
+ //encoder aan
+ counts_encoder1 = Encoder1.getPulses();
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+ counts_encoder2 = Encoder2.getPulses();
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
}
}
else{
pwm_motor2=0;
pwm_motor1=0;
+ //encoder aan
+ counts_encoder1 = Encoder1.getPulses();
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+ counts_encoder2 = Encoder2.getPulses();
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
}
-
+ pc.printf("rev_counts_motor1= %f \r\n",rev_counts_motor1);
+ pc.printf("counts_encoder 1= %f \r\n",counts_encoder1);
}
}
\ No newline at end of file