klein beginnen
Dependencies: HIDScope QEI mbed
Revision 0:be32746458fc, committed 2015-10-28
- Comitter:
- yohoo15
- Date:
- Wed Oct 28 16:13:20 2015 +0000
- Commit message:
- proberen;
Changed in this revision
diff -r 000000000000 -r be32746458fc HIDScope.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HIDScope.lib Wed Oct 28 16:13:20 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/tomlankhorst/code/HIDScope/#5020a2c0934b
diff -r 000000000000 -r be32746458fc QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Wed Oct 28 16:13:20 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa
diff -r 000000000000 -r be32746458fc main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 28 16:13:20 2015 +0000 @@ -0,0 +1,465 @@ +#include "mbed.h" +//#include "read_filter_emg.h" +//included for fabs() function +#include <math.h> +#include "HIDScope.h" +#include <iostream> +#include "QEI.h" + +//DigitalOut led_green(LED_GREEN); +//DigitalOut led_red(LED_RED); + +Serial pc(USBTX, USBRX); +AnalogIn analog_emg_left(A0); +AnalogIn analog_emg_right(A1); + +Ticker HIDScope_timer; +Ticker Filteren_timer; +Ticker aansturen; +Ticker movement_motor; + +HIDScope scope(4); + +// defining flags +volatile bool Flag_filteren = false; +volatile bool Flag_HIDScope = false; +volatile bool left_movement = false; +volatile bool right_movement = false; + + +// making function flags. +void Go_flag_filteren() +{ + Flag_filteren = true; +} + +void Go_flag_HIDScope() +{ + Flag_HIDScope = true; +} + + + +double input_left = 0 ; +double input_right = 0 ; +//double input = 0; +double filter_signal_hid_left = 0; +double filter_signal_hid_right = 0; +//double input_right = 0; +// defining threshold +double high_threshold = 2000; +double low_threshold = 1500; + +//*** making the v for everything and conquer the world*** + +//for left +//for Notchfilter +double notch_v11=0; +double notch_v21=0; +double notch_v12=0; +double notch_v22=0; +double notch_v13=0; +double notch_v23=0; + +//for highpass filter +double high_v11=0; +double high_v21=0; +double high_v12=0; +double high_v22=0; +double high_v13=0; +double high_v23=0; + +// for lowpasfilter +double low_v11=0; +double low_v21=0; +double low_v12=0; +double low_v22=0; +double low_v13=0; +double low_v23=0; + +// for moving average +double n1 = 0; +double n2 = 0; +double n3 = 0; +double n4 = 0; +double n5 = 0; + +// for right +//for Notchfilter +double notch_v11_b=0; +double notch_v21_b=0; +double notch_v12_b=0; +double notch_v22_b=0; +double notch_v13_b=0; +double notch_v23_b=0; + +//for highpass filter +double high_v11_b=0; +double high_v21_b=0; +double high_v12_b=0; +double high_v22_b=0; +double high_v13_b=0; +double high_v23_b=0; + +// for lowpasfilter +double low_v11_b=0; +double low_v21_b=0; +double low_v12_b=0; +double low_v22_b=0; +double low_v13_b=0; +double low_v23_b=0; + +// for moving average +double n1_b = 0; +double n2_b = 0; +double n3_b = 0; +double n4_b = 0; +double n5_b = 0; + + +double filter_left; +double filter_right; + +//general biquad filter that can be called in all the filter functions +double biquad(double u, double &v1, double &v2, const double a1, + const double a2, const double b0, const double b1, const double b2) +{ + double v = u - a1*v1 - a2*v2; + double y = b0*v + b1*v1 + b2*v2; + //values of v2 and v1 are updated, as they are passed by reference + //they update globally + v2 = v1; + v1 = v; + return y; +} + +double moving_average(double y, double &n1, double &n2, double &n3, double &n4, double &n5) +{ + double average = (y + n1 + n2 +n3 + n4 + n5)/5; + n5 = n4; + n4 = n3; + n3 = n2; + n2 = n1; + n1 = y; + + return average; +} +/* +double threshold(double signal, const double lowtreshold, const double hightreshold) +{ + if (signal > hightreshold) + left = true; + else if (signal <lowtreshold) + left = false; +} +*/ +//Specifying filter coefficients highpass + +/* notch filter with 3 cascaded biquads*/ +//first notch biquad +const double notch1_a1 = -1.55951422433; +const double notch1_a2 = 0.92705680308; +const double notch1_b0 = 1.00000000000; +const double notch1_b1 = -1.61854515325; +const double notch1_b2 = 1.00000000000; + +//second notch biquad +const double notch2_a1 = -1.54767435801; +const double notch2_a2 = 0.96124842048; +const double notch2_b0 = 1.00000000000; +const double notch2_b1 = -1.61854515325; +const double notch2_b2 = 1.00000000000; + +//third notch biquad +const double notch3_a1 = -1.62600366964; +const double notch3_a2 = 0.96453460373; +const double notch3_b0 = 1.00000000000; +const double notch3_b1 = -1.61854515325; +const double notch3_b2 = 1.00000000000; + +/* high pass filter consists of three cascaded biquads + blow coefficients for those three biquads */ +//first high pass biquad +const double highp1_a1 = -0.67538034389; +const double highp1_a2 = 0.12769255668; +const double highp1_b0 = 1.00000000000; +const double highp1_b1 = -2.00000000000; +const double highp1_b2 = 1.00000000000; + +//second high pass biquad +const double highp2_a1 = -0.76475499450; +const double highp2_a2 = 0.27692273367; +const double highp2_b0 = 1.00000000000; +const double highp2_b1 = -2.00000000000; +const double highp2_b2 = 1.00000000000; + +//third high pass biquad +const double highp3_a1 = -0.99216561242; +const double highp3_a2 = 0.65663360837; +const double highp3_b0 = 1.00000000000; +const double highp3_b1 = -2.00000000000; +const double highp3_b2 = 1.00000000000; + +/* lowpass filter consists of three cascaded biquads +below the coefficients for those three biquads */ +//first low pass biquad +const double lowp1_a1 = -1.05207469728; +const double lowp1_a2 = 0.28586907478; +const double lowp1_b0 = 1.00000000000; +const double lowp1_b1 = 2.00000000000; +const double lowp1_b2 = 1.00000000000; + +//second low pass biquad +const double lowp2_a1 = -1.16338171052; +const double lowp2_a2 = 0.42191097989; +const double lowp2_b0 = 1.00000000000; +const double lowp2_b1 = 2.00000000000; +const double lowp2_b2 = 1.00000000000; + +//third low pass biquad +const double lowp3_a1 = -1.42439823874; +const double lowp3_a2 = 0.74093118112; +const double lowp3_b0 = 1.00000000000; +const double lowp3_b1 = 2.00000000000; +const double lowp3_b2 = 1.00000000000; + + + +double Filteren_left(double input) +{ + + input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER + //input_right = analog_emg_right.read(); + + // notch filter + double y1 = biquad(input, notch_v11, notch_v21, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2); + double y2 = biquad(y1, notch_v12, notch_v22, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2); + double y3 = biquad(y2, notch_v13, notch_v23, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2); + + //higpass filter + double y4 = biquad(y3, high_v11, high_v21, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2); + double y5 = biquad(y4, high_v12, high_v22, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2); + double y6 = biquad(y5, high_v13, high_v23, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2); + + //rectivier + double y7 = fabs(y6); + + //lowpas filter cascade + double y8 = biquad(y7, low_v11, low_v21, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2); + double y9 = biquad(y8, low_v12, low_v22, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2); + double y10= biquad(y9, low_v13, low_v23, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2); + + // moving average + double filter_signal = moving_average(y10,n1,n2,n3,n4,n5); + + return(filter_signal); +} + +double Filteren_right(double input_b) +{ + + input_b = input_b-0.45; //FIRST SUBTRACT MEAN THEN FILTER + //input_right = analog_emg_right.read(); + + // notch filter + double y1_b = biquad(input_b, notch_v11_b, notch_v21_b, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2); + double y2_b = biquad(y1_b, notch_v12_b, notch_v22_b, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2); + double y3_b = biquad(y2_b, notch_v13_b, notch_v23_b, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2); + + //higpass filter + double y4_b = biquad(y3_b, high_v11_b, high_v21_b, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2); + double y5_b = biquad(y4_b, high_v12_b, high_v22_b, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2); + double y6_b = biquad(y5_b, high_v13_b, high_v23_b, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2); + + //rectivier + double y7_b = fabs(y6_b); + + //lowpas filter cascade + double y8_b = biquad(y7_b, low_v11_b, low_v21_b, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2); + double y9_b = biquad(y8_b, low_v12_b, low_v22_b, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2); + double y10_b= biquad(y9_b, low_v13_b, low_v23_b, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2); + + // moving average + double filter_signal_b = moving_average(y10_b,n1_b,n2_b,n3_b,n4_b,n5_b); + + return(filter_signal_b); +} + +/*************************************************************BEGIN motor control******************************************************************************************************/ +// Define pin for motor control +DigitalOut directionPin(D4); +PwmOut PWM(D5); +DigitalOut directionPin_key(D7); +PwmOut PWM_key(D6); + +DigitalIn buttonccw(PTA4); +DigitalIn buttoncw(PTC6); + +QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in) + + +// saying buttons are not pressed +const int Buttoncw_pressed = 0; +const int Buttonccw_pressed = 0; + + +// define rotation direction and begin possition +const int cw = 1; +const int ccw = 0; +double setpoint = 0; //setpoint is in pulses + + +// Controller gain proportional and intergrator +const double motor1_Kp = 4; // more or les random number. +const double motor1_Ki = 0; +const double M1_timestep = 0.01; // reason ticker works with 100 Hz. +double motor1_error_integraal = 0; // initial value of integral error +// Defining pulses per revolution (calculating pulses to rotations in degree.) +const double pulses_per_revolution = 3200 ;// motor gear is 1:1000 + +/* +double Rotation = -2; // rotation +double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree. +*/ + +// defining flags +volatile bool flag_motor = false; + +// making function flags. +void Go_flag_motor() +{ + flag_motor = true; +} + + +// To make a new setpoint +double making_setpoint(double direction) +{ + if ( cw == direction) { + setpoint = setpoint + (pulses_per_revolution/400000); + } + if ( ccw == direction) { + setpoint = setpoint - (pulses_per_revolution/400000); + } + return(setpoint); + +} + +// Reusable P controller +double PI(double error, const double Kp, const double Ki, double Ts, double &e_int) +{ + + e_int = e_int + Ts * error; + + double PI_output = (Kp * error) + (Ki * e_int); + return PI_output; +} +// Next task, measure the error and apply the output to the plant + +// control for movement left right +void motor1_Controller() +{ + double reference = setpoint; // setpoint is in pulses + double position = wheel.getPulses(); + double error_pulses = (reference - position); // calculate the error in pulses + double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn + + double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal )); + + if(error_pulses > 0) { + directionPin.write(cw); + + } else if(error_pulses < 0) { + directionPin.write(ccw); + } else { + output = 0; + } + PWM.write(output); // out of the if loop due to abs output + + +} + +/*************************************************************END motor control******************************************************************************************************/ + +void HIDScope_kijken() +{ + scope.set(0, analog_emg_left.read()); + scope.set(1, filter_signal_hid_left); + scope.set(2, analog_emg_right.read()); + scope.set(3, filter_signal_hid_right); + scope.send(); +} + + +int main() +{ + pc.printf("at the begin"); +directionPin_key.write(cw); +PWM_key.write(0); + + aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning + HIDScope_timer.attach(&Go_flag_HIDScope, 0.02);//0.02 had worked + Filteren_timer.attach(&Go_flag_filteren,0.04);// 0.04 had worked + while(1) { + + + if(Flag_filteren) { + Flag_filteren = false; + // filter left and set bool + filter_signal_hid_left = Filteren_left(analog_emg_left.read()); + + if (filter_signal_hid_left > high_threshold) { + left_movement = true; + } else if (filter_signal_hid_left < low_threshold) { + left_movement = false; + } + // make filter right and set bool + filter_signal_hid_right = Filteren_right(analog_emg_right.read()); + + if (filter_signal_hid_right > high_threshold) { + right_movement = true; + } else if (filter_signal_hid_right < low_threshold) { + right_movement = false; + } + + + } + + + + if(Flag_HIDScope) { + Flag_HIDScope = false; + HIDScope_kijken(); + + } + + if(flag_motor) { + flag_motor = false; + motor1_Controller(); + + } + + + +// Pussing buttons to get input signal + + if( left_movement and right_movement == false) { + setpoint = making_setpoint(cw); + + + } + else if(right_movement and left_movement == false ) { + setpoint = making_setpoint(ccw); + } + else if(right_movement and left_movement) { + + PWM_key.write(1); + //pc.printf("I am working"); + } else { + PWM_key.write(0); + // pc.printf("resting /n"); + } + } +} + +
diff -r 000000000000 -r be32746458fc mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Oct 28 16:13:20 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/34e6b704fe68 \ No newline at end of file