juijiu
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture1-11 by
main.cpp
- Committer:
- WouterJS
- Date:
- 2018-10-26
- Revision:
- 3:055eb9f256fc
- Parent:
- 2:fa90eaa14f99
- Child:
- 4:34ad002cb646
File content as of revision 3:055eb9f256fc:
#include "mbed.h" #include "BiQuad.h" #include "HIDScope.h" #include <stdio.h> #include <math.h> #include "QEI.h" Serial pc(USBTX,USBRX); Timer timer; float Ts = 0.002; 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); // 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; //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) enum States {failure, waiting, calib_motor, calib_emg, operational, demo}; enum Operations {rest, forward, backward, up, down}; States current_state = calib_motor; Operations movement = rest; 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 sample_timer2; 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 volatile float filteredsignal1;//the first filtered emg signal 1 volatile float filteredsignal2;//the first filtered emg signal 2 bool state_changed = false; void filterall() { //Highpass Biquad 5 Hz static BiQuad HighPass1(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354); 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) 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); float 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,emg1_input); // scope.set(2,emg1_input); // scope.set(3,emg1_input);// scope.set(4,filteredsignal1); scope.send(); // send info to HIDScope server } //////////////////// 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(); 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 waiting: do_state_waiting(); break; } motor_controller(); // Outputs data to the computer } int main() { motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz 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; timer.start(); loop_timer.attach(&loop_function, Ts); sample_timer.attach(&scopedata, Ts); sample_timer2.attach(&filterall, Ts); while (true) { } }