juijiu
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture1-11 by
Diff: main.cpp
- Revision:
- 2:fa90eaa14f99
- Parent:
- 1:a9c933f1dc71
- Child:
- 3:055eb9f256fc
diff -r a9c933f1dc71 -r fa90eaa14f99 main.cpp --- a/main.cpp Fri Oct 26 10:50:57 2018 +0000 +++ b/main.cpp Fri Oct 26 10:59:24 2018 +0000 @@ -10,11 +10,6 @@ Serial pc(USBTX,USBRX); Timer timer; float Ts = 0.002; - - -DigitalIn buttonR(D2);//rigth button on biorobotics shield -DigitalIn buttonL(D3);//left button on biorobotics shield - int sensor_sensitivity = 32; int gear_ratio = 131; float full_ratio = gear_ratio*sensor_sensitivity*4; @@ -24,10 +19,8 @@ int counts_m1 = 0; int counts_m2 = 0; - int counts_m1_prev = 0; int counts_m2_prev = 0; - float deg_m1 = 0; float deg_m2 = 0; @@ -39,7 +32,6 @@ PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2 DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt) - enum States {failure, waiting, calib_motor, calib_emg, operational, demo}; enum Operations {rest, forward, backward, up, down}; @@ -53,11 +45,9 @@ float thresholdtime = 1.0; // time waiting before switching modes Ticker loop_timer; - Ticker sample_timer; Ticker sample_timer2; - -HIDScope scope( 5 ); +HIDScope scope(5); AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength @@ -72,22 +62,20 @@ volatile float filteredsignal2;//the first filtered emg signal 2 bool state_changed = false; -float high1; -float abs1; -float low1; + void filterall() { //Highpass Biquad 5 Hz static BiQuad HighPass1(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354); -high1 = HighPass1.step(emg1_input); +float high1 = HighPass1.step(emg1_input); static BiQuad HighPass2(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354); float high2 = HighPass2.step(emg2_input); // Rectify the signal(absolute value) -abs1 = fabs(high1); +float abs1 = fabs(high1); float abs2 = fabs(high2); //Lowpass Biquad 10 Hz static BiQuad LowPass1(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906); -low1 = LowPass1.step(abs1); +float low1 = LowPass1.step(abs1); static BiQuad LowPass2(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906); float low2 = LowPass2.step(abs2); @@ -117,15 +105,12 @@ } - - - void scopedata() { scope.set(0,emg1_input); // - scope.set(1,high1); // - scope.set(2,abs1); // - scope.set(3,low1);// + scope.set(1,emg1_input); // + scope.set(2,emg1_input); // + scope.set(3,emg1_input);// scope.set(4,filteredsignal1); scope.send(); // send info to HIDScope server } @@ -211,10 +196,7 @@ if(on){ timer.reset(); - motor1_direction = 1; //set motor 1 to run counterclockwise (positive) direction for calibration - motor1_speed_control = 0.3;//to make sure the motor will not run at too high velocity - motor2_direction = 0; // set motor 2 to run clockwise (negative) direction - motor2_speed_control = 0.3; + on = false; } @@ -347,19 +329,16 @@ int main() { motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz - - - - timer.start(); - loop_timer.attach(&loop_function, Ts); - sample_timer.attach(&scopedata, Ts); - sample_timer2.attach(&filterall, Ts); + + //timer.start(); + //loop_timer.attach(&loop_function, Ts); + //sample_timer.attach(&scopedata, Ts); + //sample_timer2.attach(&filterall, Ts); - - - - //led_red = 1; - //led_green = 1; - while (true) { + while (true) { + motor1_direction = 1; //set motor 1 to run counterclockwise (positive) direction for calibration + motor1_speed_control = 0.55;//to make sure the motor will not run at too high velocity + motor2_direction = 0; // set motor 2 to run clockwise (negative) direction + motor2_speed_control = 0.55; } } \ No newline at end of file