lololololol
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 30:a16519224d58
- Parent:
- 29:5a846abba59e
- Child:
- 31:465a6e1e1db6
--- a/main.cpp Wed Oct 30 11:05:31 2019 +0000 +++ b/main.cpp Wed Oct 30 14:43:34 2019 +0000 @@ -49,16 +49,12 @@ volatile bool pressed_2 = false; HIDScope scope(6); -//volatile float theta_error_1; volatile float theta_ref1; -//volatile float theta_error_2; volatile float theta_ref2; float Ts = 0.01; float Kp; float Ki; float Kd; -float thetav_1; -float thetav_2; float theta_1 = (40.0f*pi)/180.0f; float theta_2 = (175.0f*pi)/180.0f; float theta_error1; @@ -71,6 +67,7 @@ volatile float EMGy_velocity=0; char beweging; + BiQuad Lowpass_bl ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 ); BiQuad Highpass_bl ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00); BiQuad notch_bl (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01); @@ -157,13 +154,6 @@ led_red=1; } - -void CalculateError() -{ - theta_error1 = theta_ref1-theta_1; - theta_error2 = theta_ref2-theta_2; -} - void Controller() { float K = 1; @@ -173,6 +163,9 @@ Ki = K/ti; Kd = K*td; + theta_error1 = theta_ref1-theta_1; + theta_error2 = theta_ref2-theta_2; + float error_integral1 = 0; float error_integral2 = 0; float error_prev1 = 0; @@ -202,30 +195,25 @@ // Sum all parts and return it torque_1 = torque_p1 + torque_i1 + torque_d1; torque_2 = torque_p2 + torque_i2 + torque_d2; - } -void AngleVelocity() +void Kinematics() { float DET_jacobian= 1.0f/((-l*sin(theta_1)-l*sin(theta_1+theta_2))*(l*cos(theta_1+theta_2))-(-l*sin(theta_1+theta_2))*(l*cos(theta_1)+l*cos(theta_1+theta_2))); - thetav_1=DET_jacobian*l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*l*sin(theta_1+theta_2)*EMGy_velocity; - thetav_2= DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)*EMGy_velocity; -} - -void JointAngle() -{ + float thetav_1=DET_jacobian*l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*l*sin(theta_1+theta_2)*EMGy_velocity; + float thetav_2= DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)*EMGy_velocity; theta_ref1=theta_1+thetav_1*Ts; theta_ref2=theta_2+thetav_2*Ts; x=cos(theta_ref1)*l+cos(theta_ref1+theta_ref2)*l; y=sin(theta_ref1)*l+sin(theta_ref1+theta_ref2)*l; if (sqrt(pow(x,2)+pow(y,2))>1.0f) { - theta_1=theta_ref1; - theta_2=theta_ref2; + theta_ref1 = theta_1; + theta_ref2 = theta_2; } } - void CalculateDirectionMotor() { + Kinematics(); Controller(); direction_motor_1 = torque_1 <= 0.0f; direction_motor_2 = torque_2 <= 0.0f; @@ -233,17 +221,16 @@ void ReadEncoder() { - theta_1 = ((360.0f/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360. - theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f; + theta_1 = (((2.0f*pi)/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360. + theta_2 = (((2.0f*pi)/64.0f)*(float)encoder_2.getPulses())/131.25f; } void MotorControl() { - Controller(); ReadEncoder(); CalculateDirectionMotor(); - PWM_motor_1.write(fabs(torque_1)/360.0f); - PWM_motor_2.write(fabs(torque_2)/360.0f); + PWM_motor_1.write(fabs(torque_1)/(2.0f*pi)); + PWM_motor_2.write(fabs(torque_2)/(2.0f*pi)); } void go_next_1() @@ -569,13 +556,19 @@ CurrentState = horizontal_movement; StateChanged = true; }*/ - if (beweging =='s') { + + while (beweging =='s') { EMGy_velocity = -0.02f; + pc.printf("beweging %c \n\r", beweging); + pc.printf("beweging %f \n\r", EMGy_velocity); + } - else if (beweging == 'w') { + while (beweging == 'w') { EMGy_velocity = 0.02f; + pc.printf("beweging %c \n\r", beweging); + pc.printf("beweging %f \n\r", EMGy_velocity); } - else { + while(beweging != 's' || beweging !='w'){ EMGy_velocity = 0.0f; } if ((pressed_1) || (pressed_2) /*|| (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))*/) { // EMG gebaseerde threshold aanmaken @@ -606,8 +599,9 @@ EMGx_velocity = -0.02f; pc.printf(" you pressed %c \n\r" , beweging); } - else if (beweging == 'd') { + if (beweging == 'd') { EMGx_velocity = 0.02f; + pc.printf(" you pressed %c \n\r" , beweging); } else { EMGx_velocity = 0.0f; @@ -691,6 +685,11 @@ PWM_motor_2.period_ms(10); motor_control.attach(&MotorControl, Ts); write_scope.attach(&WriteScope, 0.01); + + void CalculateDirectionMotor(); + void MotorControl(); + + TickerStateMachine.attach(StateMachine,1.00f); beweging = pc.getc(); while(true) {