juijiu
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture1-11 by
main.cpp
- Committer:
- WouterJS
- Date:
- 2018-10-29
- Revision:
- 4:34ad002cb646
- Parent:
- 3:055eb9f256fc
- Child:
- 5:892531e4e015
File content as of revision 4:34ad002cb646:
#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); // DigitalOut led_red(LED_RED); 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; 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) float kp1 = 2; float kp2 = 2; float ki1 = 0.5; float ki2 = 0.5; float u1 = 0; float u2 = 0; float ref_q1 = 0; float ref_q2 = 0; float L0 = 0.1; float L1 = 0.1; float L2 = 0.4; float ref_v1; float ref_v2; enum States {failure, waiting, calib_motor, homing ,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(){ //twist1, 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(){ } int count1 = 0; int count2 = 0; void do_state_calib_motor(){ if (state_changed==true) { state_changed = false; } int deriv1 = deg_m1 - count1; int deriv2 = deg_m2 - count2; count1 = deg_m1; count2 = deg_m2; if ( timer.read() > 3 && deriv1 < 0.5 && deriv2 < 0.5) { motor1_speed_control = 0; motor2_speed_control = 0; current_state = homing; timer.reset(); state_changed = true; deg_m1 = -45; deg_m2 = -70; } } void do_state_homing(){ if (state_changed==true) { state_changed = false; } float werror1 = deg_m1 - 0; float werror2 = deg_m2 - 0; float wu1 = kp1*werror1 + (u1 + werror1*0.002)*ki1; float wu2 = kp2*werror1 + (u2 + werror1*0.002)*ki2; motor1_speed_control = fabs(wu1/200); if(wu1 > 0){ motor1_direction = 0;} if(wu1< 0){ motor1_direction = 1;} motor2_speed_control = fabs(wu2/200); if(wu2 > 0){ motor2_direction = 0;} if(wu2< 0){ motor2_direction = 1;} } 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 jacobian[4]; float inv_jacobian[4]; jacobian[0] = L1; jacobian[1] = L2*sin(deg_m1)+L1; jacobian[2] = -L0; jacobian[3] = -L0 - L2*cos(deg_m1); float det = 1/(jacobian[0]*jacobian[3]-jacobian[1]*jacobian[2]); inv_jacobian[0] = det*jacobian[3]; inv_jacobian[1] = -det*jacobian[1]; inv_jacobian[2] = -det*jacobian[2]; inv_jacobian[3] = det*jacobian[0]; //ref_v1 = jacobian[0]*twist[0]+jacobian[1]*twist[1]; //ref_v2 = jacobian[2]*twist[0]+jacobian[3]*twist[1]; //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); //ref_q1 = ref_q1 + 0.002*ref_v1; //ref_q2 = ref_q2 + 0.002*ref_v2; //float error1 = deg_m1 - ref_q1; //float error2 = deg_m2 - ref_q2; //u1 = kp1*error1 + (u1 + error1*0.002)*ki1; //u2 = kp2*error1 + (u2 + error1*0.002)*ki2; } 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 homing: do_state_homing(); 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.25;//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.25; led_red = 0; timer.start(); loop_timer.attach(&loop_function, Ts); sample_timer.attach(&scopedata, Ts); sample_timer2.attach(&filterall, Ts); while (true) { } }