juijiu
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture1-11 by
main.cpp
- Committer:
- thijslubberman
- Date:
- 2018-11-01
- Revision:
- 11:c7e27de26ac0
- Parent:
- 10:91173ed1e841
- Child:
- 12:c59b25d07bb9
File content as of revision 11:c7e27de26ac0:
#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; double pi = 3.141592653589793238462643383279502884; DigitalIn but1(D2); 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); DigitalOut led_green(LED_GREEN); DigitalOut led_blue(LED_BLUE); 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 (0 is CCW ) PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1 PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2 DigitalOut motor2_direction(D7);// draairichting motor 2 (0 is CCW ) float int_u1 = 0; float int_u2 = 0; 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; float teraterm1; float teraterm2; float error1; float error2; float filtered_d_error1; float filtered_d_error2; float kp1 = 0.1; float kp2 = 0.1; float ki1 = 0.05; float ki2 = 0.05; float kd1 = 0.005; float kd2 = 0.005; float olderror1; float olderror2; float d_error1; float d_error2; 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(3); 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; static BiQuad Notch1(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592); static BiQuad Notch2(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592); static BiQuad HighPass1(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354); static BiQuad HighPass2(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354); static BiQuad LowPass1(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906); static BiQuad LowPass2(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906); static BiQuad LowpassFilter1(0.0640,0.1279,0.0640,-1.1683,0.4241); static BiQuad LowpassFilter2(0.0640,0.1279,0.0640,-1.1683,0.4241); void filterall() { //Notch 50 Hz BW 6 Hz float notch1 = Notch1.step(emg1_input); float notch2 = Notch2.step(emg2_input); //Highpass Biquad 5 Hz float high1 = HighPass1.step(notch1); float high2 = HighPass2.step(notch2); // Rectify the signal(absolute value) float abs1 = fabs(high1); float abs2 = fabs(high2); //Lowpass Biquad 10 Hz float low1 = LowPass1.step(abs1); 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 = Encoder2.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,filteredsignal1); // //scope.set(1,filteredsignal2); // //scope.set(2,ref_q1); // // scope.set(3,movement);// // scope.set(4,ref_q1); //scope.send(); // send info to HIDScope server } //////////////////// MOVEMENT STATES float twist[2] = {0,0}; float twistf[2] = {0,0}; float abs_sig; int gain = 3; int sign1 = 0.5; int sign2 = 1; void do_forward(){ sign1 = 0.5; sign2 = 1; twistf[0] = 1; twistf[1] = 0; if (filteredsignal2 > (0.3*max2)){ abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2)* gain; } else { abs_sig = 0; } twist[0] = twistf[0] * abs_sig ; twist[1] = twistf[1] * abs_sig ; motor1_speed_control = fabs(u1); if(u1 > 0){ motor1_direction = 1;} if(u1 < 0){ motor1_direction = 0;} motor2_speed_control = fabs(u2); if(u2 > 0){ motor2_direction = 1;} if(u2 < 0){ motor2_direction = 0;} if( timer.read() > thresholdtime && filteredsignal1 > threshold1) { movement = backward; timer.reset(); d_error1 = 0; d_error2 = 0; u1 = 0; u2 = 0; int_u1 = 0; int_u2 = 0; led_green = 0; } } void do_backward(){ sign1 = -1; sign2 = 1; twistf[0] = -1; twistf[1] = 0; if (filteredsignal2 > (0.3*max2)){ abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2)* gain; } else { abs_sig = 0; } twist[0] = twistf[0] * abs_sig ; twist[1] = twistf[1] * abs_sig ; motor1_speed_control = fabs(u1); if(u1 > 0){ motor1_direction = 1;} if(u1 < 0){ motor1_direction = 0;} motor2_speed_control = fabs(u2); if(u2 > 0){ motor2_direction = 1;} if(u2 < 0){ motor2_direction = 0;} if( timer.read() > thresholdtime && filteredsignal1 > threshold1) { movement = up; timer.reset(); d_error1 = 0; d_error2 = 0; u1 = 0; u2 = 0; int_u1 = 0; int_u2 = 0; led_red = 1; } } void do_up(){ sign1 = 1; sign2 = 1; twistf[0] = 0; twistf[1] = 1; if (filteredsignal2 > (0.3*max2)){ abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2)* gain; } else { abs_sig = 0; } twist[0] = twistf[0] * abs_sig ; twist[1] = twistf[1] * abs_sig ; motor1_speed_control = fabs(u1); if(u1 > 0){ motor1_direction = 1;} if(u1 < 0){ motor1_direction = 0;} motor2_speed_control = fabs(u2); if(u2 > 0){ motor2_direction = 1;} if(u2 < 0){ motor2_direction = 0;} if( timer.read() > thresholdtime && filteredsignal1 > threshold1) { movement = down; timer.reset(); d_error1 = 0; d_error2 = 0; u1 = 0; u2 = 0; int_u1 = 0; int_u2 = 0; led_green = 1; led_blue = 0; } } void do_down(){ sign1 = 1; sign2 = -1; twistf[0] = 0; twistf[1] = -1; if (filteredsignal2 > (0.3*max2)){ abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2)* gain; } else { abs_sig = 0; } twist[0] = twistf[0] * abs_sig ; twist[1] = twistf[1] * abs_sig ; motor1_speed_control = fabs(u1); if(u1 > 0){ motor1_direction = 1;} if(u1 < 0){ motor1_direction = 0;} motor2_speed_control = fabs(u2); if(u2 > 0){ motor2_direction = 1;} if(u2 < 0){ motor2_direction = 0;} if( timer.read() > thresholdtime && filteredsignal1 > threshold1) { movement = rest; timer.reset(); d_error1 = 0; d_error2 = 0; u1 = 0; u2 = 0; int_u1 = 0; int_u2 = 0; led_red = 0; led_green = 0; } } void do_wait(){ if( timer.read() > thresholdtime && filteredsignal1 > threshold1) { movement = forward; timer.reset(); led_green = 1; led_blue = 1; } } ///////////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; } led_red = 0; 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; state_changed = true; wait(1); deg_m1 = 57.4; deg_m2 = 72.93; led_red = 1; led_green = 0; } } float wu1; float wu2; void do_state_homing(){ if (state_changed==true) { timer.reset(); state_changed = false; } float kp11 = 0.1; float kp12 = 0.1; float werror1 = deg_m1 - 0; float werror2 = deg_m2 - 0; if(werror1 > 5){ wu1 = 1; } if(werror1 < -5){ wu1 = -1; } else{ wu1 = kp11*werror1; //+ (u1 + werror1*0.002)*ki1; } if(werror2 > 5){ wu2 = 1;} if(werror2 < -5){ wu2 = -1;} else{ wu2 = kp12*werror2; //+ (u2 + werror2*0.002)*ki2); } motor1_speed_control = fabs(wu1)/16; if(wu1 > 0){ motor1_direction = 1;} if(wu1< 0){ motor1_direction = 0;} motor2_speed_control = fabs(wu2)/16; if(wu2 > 0){ motor2_direction = 1;} if(wu2 < 0){ motor2_direction = 0;} if ( timer.read() > 4) { motor1_speed_control = 0; motor2_speed_control = 0; ref_q1 = 0; ref_q2 = 0; deg_m1 = 0; deg_m2 = 0; d_error1 = 0; d_error2 = 0; u1 = 0; u2 = 0; int_u1 = 0; int_u2 = 0; wait(1); current_state = calib_emg; timer.reset(); state_changed = true; led_green = 1; led_blue = 0; } } 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() > 5 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) { current_state = operational; timer.reset(); state_changed = true; led_green = 0; led_red = 0; } } 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; } } void do_state_waiting(){ if (state_changed==true) { state_changed = false; } if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) { current_state = operational; state_changed = true; movement = rest; led_green = 0; led_blue = 0; led_red = 0; } } //////////////// END ROBOT ARM STATES ////////////////////////////// void motor_controller(){ float jacobian[4]; float inv_jacobian[4]; jacobian[0] = L1; jacobian[1] = (L2*cos(deg_m2/180*pi))+L1; jacobian[2] = -L0; jacobian[3] = -L0 - (L2*sin(deg_m2/180*pi)); 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 = inv_jacobian[0]*twist[0]+inv_jacobian[1]*twist[1]; ref_v2 = inv_jacobian[2]*twist[0]+inv_jacobian[3]*twist[1]; ref_q1 = ref_q1 + 0.002*ref_v1 * sign1; ref_q2 = ref_q2 + 0.002*ref_v2 * sign2; error1 = deg_m1 - ref_q1; error2 = deg_m2 - ref_q2; // if(error1 > 5){ // u1 = 1; } //if(error1 < -5){ // u1 = -1; } //else{ d_error1 = (error1 - olderror1)/Ts; filtered_d_error1 = LowpassFilter1.step(d_error1); int_u1 = int_u1 + error1 * Ts; u1 = kp1*error1 + int_u1*ki1 + kd1*filtered_d_error1; teraterm1 = u1; // to see how big u1 and u2 actually get, they should lie between -1 and 1 for the right gains. if(u1>1){ u1 =1; } if(u1<-1){ u1 = -1; } //} olderror1 = error1; //if(error2 > 5){ // u2 = 1;} // if(error2 < -5){ // u2 = -1;} //else{ d_error2 = (error2 - olderror2)/Ts; filtered_d_error2 = LowpassFilter2.step(d_error2); int_u2 = int_u2 + error2 * Ts; u2 = kp2*error2 + int_u2*ki2 + kd2*filtered_d_error2; teraterm2 = u2; if(u2>1){ u2 =1; } if(u2<-1){ u2 = -1; } // } olderror2 = error2; } 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 = 0; //set motor 1 to run clockwisedirection for calibration motor1_speed_control = 0.15;//to make sure the motor will not run at too high velocity motor2_direction = 0; // set motor 2 to run clockwise direction motor2_speed_control = 0.15; led_red = 1; led_green = 1; led_blue = 1; timer.start(); loop_timer.attach(&loop_function, Ts); sample_timer.attach(&scopedata, Ts); sample_timer2.attach(&filterall, Ts); pc.baud(115200); while (true) { printf("%i %i %f %f %f %f \n\r",movement,current_state,error1,error2,deg_m1, deg_m2); } }