juijiu
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture1-11 by
main.cpp
- Committer:
- thijslubberman
- Date:
- 2018-10-31
- Revision:
- 8:c7d21f5f87d8
- Parent:
- 7:db050a878cff
- Child:
- 9:4f594927cff3
File content as of revision 8:c7d21f5f87d8:
#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; 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 kp1 = 0.1; float kp2 = 0.1; float ki1 = 0.01; float ki2 = 0.01; float kd1 = 0.0005; float kd2 = 0.0005; 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; 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 = forward; 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 LowpassFilter(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; void do_forward(){ twistf[0] = 1; twistf[1] = 0; if (filteredsignal2 > 0.15*max2){ abs_sig = (filteredsignal2 - (0.15*max2))/(0.85*max2); } else { abs_sig = 0; } //if (but1 == false){ // abs_sig = 0.75; // } twist[0] = twistf[0] * abs_sig* gain; twist[1] = twistf[1] * abs_sig* gain; 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*100) // { // 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; } 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 werror1 = deg_m1 - 0; float werror2 = deg_m2 - 0; if(werror1 > 5){ wu1 = 1; } if(werror1 < -5){ wu1 = -1; } else{ wu1 = kp1*werror1; //+ (u1 + werror1*0.002)*ki1; } if(werror2 > 5){ wu2 = 1;} if(werror2 < -5){ wu2 = -1;} else{ wu2 = kp2*werror2; //+ (u2 + werror2*0.002)*ki2); } motor1_speed_control = fabs(wu1)/8; if(wu1 > 0){ motor1_direction = 1;} if(wu1< 0){ motor1_direction = 0;} motor2_speed_control = fabs(wu2)/8; if(wu2 > 0){ motor2_direction = 1;} if(wu2 < 0){ motor2_direction = 0;} if ( timer.read() > 4) { motor1_speed_control = 0; motor2_speed_control = 0; deg_m1 = 0; deg_m2 = 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; } 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_m2)+L1; jacobian[2] = -L0; jacobian[3] = -L0 - L2*cos(deg_m2); 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; ref_q2 = ref_q2 + 0.002*ref_v2; error1 = deg_m1 - ref_q1; error2 = deg_m2 - ref_q2; float olderror1; float olderror2; float d_error1; float d_error2; if(error1 > 5){ u1 = 1; } if(error1 < -5){ u1 = -1; } else{ d_error1 = (error1 - olderror1)/Ts; filtered_d_error1 = LowpassFilter.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 = LowpassFilter.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; } } } 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); while (true) { printf("%f %f %f %f %f \n\r",ref_q1,ref_q2,error1,int_u1,filtered_d_error1); } }