juijiu
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture1-11 by
Revision 1:a9c933f1dc71, committed 2018-10-26
- Comitter:
- WouterJS
- Date:
- Fri Oct 26 10:50:57 2018 +0000
- Parent:
- 0:3710031b2621
- Child:
- 2:fa90eaa14f99
- Commit message:
- update!;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eigen.lib Fri Oct 26 10:50:57 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/ykuroda/code/Eigen/#13a5d365ba16
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Oct 26 10:50:57 2018 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- a/main.cpp Fri Oct 19 10:37:25 2018 +0000 +++ b/main.cpp Fri Oct 26 10:50:57 2018 +0000 @@ -1,60 +1,70 @@ #include "mbed.h" #include "BiQuad.h" #include "HIDScope.h" -#include <sys/time.h> +#include <stdio.h> +#include <math.h> +#include <iostream> +#include "QEI.h" + + +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 -class Timer -{ -private: - struct timeval start_t; -public: - double start() { gettimeofday(&start_t, NULL); } - double get_ms() { - struct timeval now; - gettimeofday(&now, NULL); - return (now.tv_usec-start_t.tv_usec)/(double)1000.0 + - (now.tv_sec-start_t.tv_sec)*(double)1000.0; - } - double get_ms_reset() { - double res = get_ms(); - reset(); - return res; - } - Timer() { start(); } -}; +int sensor_sensitivity = 32; +int gear_ratio = 131; +float full_ratio = gear_ratio*sensor_sensitivity*4; + +QEI Encoder1(D10,D11,NC,sensor_sensitivity); //First one is B, Second one is A +QEI Encoder2(D12,D13,NC,sensor_sensitivity); // -Timer t(); -double used_ms; -Serial pc(USBTX,USBRX);// serial connection to pc +int counts_m1 = 0; +int counts_m2 = 0; + +int counts_m1_prev = 0; +int counts_m2_prev = 0; -DigitalOut led_red(LED_RED); -DigitalOut led_green(LED_RED); -DigitalIn buttonR(D2);//rigth button on biorobotics shield -DigitalIn buttonL(D3);//rigth button on biorobotics shield +float deg_m1 = 0; +float deg_m2 = 0; + +//Vector2d twist(0,0); +//Matrix2f jacobian(0, 0, 0, 0), inv_jacobian(0, 0, 0, 0); DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt) PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1 - +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) -PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2 + -enum States {failure, wait, calib_emg, operational, demo}; +enum States {failure, waiting, calib_motor, calib_emg, operational, demo}; enum Operations {rest, forward, backward, up, down}; -States current_state; +States current_state = calib_motor; Operations movement = rest; -float max1 = 0.3; //initial threshold value for emg signals, changes during calibration -float max2 = 0.3; +float max1 = 0; //initial threshold value for emg signals, changes during calibration left arm +float max2 = 0; // right arm +float threshold1; +float threshold2; +float thresholdtime = 1.0; // time waiting before switching modes + +Ticker loop_timer; Ticker sample_timer; -Ticker loop_timer; +Ticker sample_timer2; -HIDScope scope( 3 ); +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 +volatile float emg1_input; +volatile float emg2_input; + volatile float raw_filteredsignal1;//the first filtered emg signal 1 volatile float raw_filteredsignal2;//the first filtered emg signal 2 @@ -62,168 +72,294 @@ volatile float filteredsignal2;//the first filtered emg signal 2 bool state_changed = false; - -measureall(){ // changes all variables according in sync with the rest of the code - - emg1_input = emg1_input.read(); - emg2_input = emg2_input.read(); - filterall(); - filteredsignal1 = raw_filteredsignal1; - filteredsignal2 = raw_filteredsignal2; - //Reading of motor - - - -} - - +float high1; +float abs1; +float low1; void filterall() { //Highpass Biquad 5 Hz -static BiQuad HighPass(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354); -float high1 = HighPass.step(raw_emg1_input); -float high2 = HighPass.step(raw_emg2_input); +static BiQuad HighPass1(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354); +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) -float abs1 = fabs(high1); +abs1 = fabs(high1); float abs2 = fabs(high2); //Lowpass Biquad 10 Hz -static BiQuad LowPass(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906); -float low1 = LowPass.step(abs1); -float low2 = LowPass.step(abs2); +static BiQuad LowPass1(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906); +low1 = LowPass1.step(abs1); +static BiQuad LowPass2(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906); +float low2 = LowPass2.step(abs2); raw_filteredsignal1 = low1; raw_filteredsignal2 = low2; + } +void measureall(){ // changes all variables according in sync with the rest of the code + + emg1_input = raw_emg1_input.read(); + emg2_input = raw_emg2_input.read(); + + filteredsignal1 = raw_filteredsignal1; + filteredsignal2 = raw_filteredsignal2; + + //Reading of motor + + counts_m1 = Encoder1.getPulses() - counts_m1_prev; + counts_m2 = Encoder1.getPulses() - counts_m2_prev; + deg_m1 = deg_m1 + counts_m1*(360/(full_ratio)); + deg_m2 = deg_m2 + counts_m2*(360/(full_ratio)); + counts_m1_prev = Encoder1.getPulses(); + counts_m2_prev = Encoder2.getPulses(); + + +} + + + + void scopedata() { scope.set(0,emg1_input); // - scope.set(1,filteredsignal1); // - scope.set(2,filteredsignal2); // + scope.set(1,high1); // + scope.set(2,abs1); // + scope.set(3,low1);// + scope.set(4,filteredsignal1); scope.send(); // send info to HIDScope server } -void loop_function() { + + //////////////////// MOVEMENT STATES +void do_forward(){ + + //twist << 1, 0; + //Vector2d twistf(0,0); + //twistf << 1, 0; + if (filteredsignal2 > threshold2){ + //double abs_sig = (filteredsignal2 - (0.5*max2))/(0.5*max2); + + //twist = twistf * abs_sig; + + } + + + if( timer.read() > thresholdtime && filteredsignal1 > threshold1) + { + movement = backward; + timer.reset(); + } + } + +void do_backward(){ + + + + if( timer.read() > thresholdtime && filteredsignal1 > threshold1) + { + movement = up; + timer.reset(); + } + } + +void do_up(){ + + //Code for moving up + + if( timer.read() > thresholdtime && filteredsignal1 > threshold1) + { + movement = down; + timer.reset(); + } + } +void do_down(){ + + //Code for moving down + + if( timer.read() > thresholdtime && filteredsignal1 > threshold1) + { + movement = rest; + timer.reset(); + + } + } +void do_wait(){ + + if ( filteredsignal2 > threshold2) {// + current_state = waiting; + state_changed = true; + } + if( timer.read() > thresholdtime && filteredsignal1 > threshold1) + { + movement = forward; + timer.reset(); + } + } +///////////END MOVEMENT STATES///////////////////////// +///////////ROBOT ARM STATES /////////////////////////// + +void do_state_failure(){ + + } + bool on = true; +void do_state_calib_motor(){ + if (state_changed==true) { + state_changed = false; + } + + + 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; + } + + int deriv1 = fabs((counts_m1 - counts_m1_prev)/Ts); + int deriv2 = fabs((counts_m2 - counts_m2_prev)/Ts); + + + if ( timer.read() > 5 && deriv1 < 1 && deriv2 < 1) { + motor1_speed_control = 0; + motor2_speed_control = 0; + current_state = calib_emg; + timer.reset(); + state_changed = true; + } + } +void do_state_calib_emg(){ + if (state_changed==true) { + state_changed = false; + } + + if(filteredsignal1 > max1){//calibrate to a new maximum + max1 = filteredsignal1; + threshold1 = 0.5*max1; + } + if(filteredsignal2 > max2){//calibrate to a new maximum + max2 = filteredsignal2; + threshold2 = 0.5 * max2; + } + + if (timer.read() > 10 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) { + current_state = operational; + timer.reset(); + state_changed = true; + } +} + +void do_state_operational(){ + if (state_changed==true) { + state_changed = false; + } + + switch(movement) {// a separate function for what happens in each state + case rest: + do_wait(); + break; + case forward: + do_forward(); + + break; + case backward: + do_backward(); + + break; + case up: + do_up(); + break; + case down: + do_down(); + break; + } + if (movement == rest && filteredsignal2 > threshold2) { + current_state = waiting; + state_changed = true; + } + +} + +void do_state_waiting(){ + if (state_changed==true) { + state_changed = false; + } + + if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) { + current_state = operational; + state_changed = true; + } +} +//////////////// END ROBOT ARM STATES ////////////////////////////// + +void motor_controller(){ + float q1; + float q2; + //q1 defined + //q2 defined + + //float L0 = 0.1; + //float L1 = 0.1; + //float L2 = 0.4; + + //jacobian << L1, L2*sin(q1)+L1,-L0,-L0 - L2*cos(q1); + + //inv_jacobian = jacobian.inverse(); + //Vector2d reference_vector = inv_jacobian*twist; + //float ref_v1 = reference_vector(0); + //float ref_v2 = reference_vector(1); + + //float ref_q1 = 0; + //ref_q1 = ref_p1 + 0.002*ref_v1; + // float ref_q2 = 0; + //ref_q2 = ref_p2 + 0.002*ref_v2; + + + + } + +void loop_function() { //Main loop function measureall(); switch(current_state) { case failure: do_state_failure(); // a separate function for what happens in each state break; + case calib_motor: + do_state_calib_motor(); + break ; case calib_emg: do_state_calib_emg(); break; case operational: do_state_operational(); break; - case wait; - do_state_wait(); + case waiting: + do_state_waiting(); break; } motor_controller(); -scopedata(); // Outputs data to the computer -} - -do_state_failure(){ - //al motor movement to zero! - wait(1000); - }; - -do_state_calib_emg(){ - if (state_changed==true) { - state_changed = false; - } - if(filteredsignal1 > max1){//calibrate to a new maximum - max1 = filteredsignal1; - } - if(filteredsignal2 > max2){//calibrate to a new maximum - max2 = filteredsignal2; - } - - if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2)) { - current_state = operational; - used_ms = t.get_ms_reset(); - state_changed = true; - } -} - -do_state_operational(){ - if (state_changed==true) { - state_changed = false; - } - - switch(States) {// a separate function for what happens in each state - case rest: - if (filteredsignal2 > (0.6*max))) {// - current_state = wait; - state_changed = true; - } - if( t.get_ms() > 1000 && filteredsignal1 > (0.6max)) - { - States = forward; - used_ms = t.get_ms_reset(); - } - break; - case forward: - do_forward(); - if( t.get_ms() > 1000 && filteredsignal1 > (0.6max)) - { - States = backward; - used_ms = t.get_ms_reset(); - } - break; - case backward: - do_backward(); - if( t.get_ms() > 1000 && filteredsignal1 > (0.6max)) - { - States = up; - used_ms = t.get_ms_reset(); - } - break; - case up: - do_up(); - if( t.get_ms() > 1000 && filteredsignal1 > (0.6max)) - { - States = wait; - used_ms = t.get_ms_reset(); - } - break; - case down: - do_down(); - if( t.get_ms() > 1000 && filteredsignal1 > (0.6max)) - { - States = wait; - used_ms = t.get_ms_reset(); - } - break; - } - - -} - -do_state_wait(){ - if (state_changed==true) { - state_changed = false; - } - - if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2) { - current_state = operational; - state_changed = true; - } +// Outputs data to the computer } - int main() -{ - - loop_timer.attach(&loop_function, 0.002); +{ + motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz + + - pc.baud(115200); + timer.start(); + loop_timer.attach(&loop_function, Ts); + sample_timer.attach(&scopedata, Ts); + sample_timer2.attach(&filterall, Ts); + - while (true) { - if(buttonR == true){ - current_state = failure; - } + + + //led_red = 1; + //led_green = 1; + while (true) { } } \ No newline at end of file