lololololol
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 35:ab9b1c9b6d08
- Parent:
- 34:89a424fd37ce
- Child:
- 36:66f500e387c4
diff -r 89a424fd37ce -r ab9b1c9b6d08 main.cpp --- a/main.cpp Thu Oct 31 13:06:38 2019 +0000 +++ b/main.cpp Thu Oct 31 17:44:11 2019 +0000 @@ -54,9 +54,12 @@ volatile float theta_ref1; volatile float theta_ref2; float Ts = 0.01f; -float Kp; -float Ki; -float Kd; +float Kp0; +float Ki0; +float Kd0; +float Kp1; +float Ki1; +float Kd1; float theta_1 = (40.0f*pi)/180.0f; float theta_2 = (175.0f*pi)/180.0f; float theta_error1; @@ -193,40 +196,62 @@ float Controller(float theta_error, bool motor) { if (motor == false) { - float K = 1; - float ti = 0.1; - float td = 10; - Kp = K*(1+td/ti); - Ki = K/ti; - Kd = K*td; + float K0 = 75.0f; + float ti0 = 5.0f; + float td0 = 0.1f; + Kp0 = K0*(1+td0/ti0); + Ki0 = K0/ti0; + Kd0 = K0*td0; + + static float error_integral0 = 0; + static float error_prev0 = 0; + static BiQuad LowPassFilter0(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + + // Proportional part: + float torque_p0 = Kp0 * theta_error; + + // Integral part: + error_integral0 = error_integral0 + theta_error * Ts; + float torque_i0 = Ki0 * error_integral0; + + // Derivative part: + float error_derivative0 = (theta_error - error_prev0)/Ts; + float filtered_error_derivative0 = LowPassFilter0.step(error_derivative0); + float torque_d0 = Kd0 * filtered_error_derivative0; + error_prev0 = theta_error; + + // Sum all parts and return it + float torque0 = torque_p0 + torque_i0 + torque_d0; + return torque0; } else { - float K = 1; - float ti = 0.1; - float td = 10; - Kp = K*(1+td/ti); - Ki = K/ti; - Kd = K*td; + float K1 = 75.0f; + float ti1 = 1.0f; + float td1 = 0.01f; + Kp1 = K1*(1+td1/ti1); + Ki1 = K1/ti1; + Kd1 = K1*td1; + + static float error_integral1 = 0; + static float error_prev1 = 0; + static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + + // Proportional part: + float torque_p1 = Kp1 * theta_error; + + // Integral part: + error_integral1 = error_integral1 + theta_error * Ts; + float torque_i1 = Ki1 * error_integral1; + + // Derivative part: + float error_derivative1 = (theta_error - error_prev1)/Ts; + float filtered_error_derivative1 = LowPassFilter1.step(error_derivative1); + float torque_d1 = Kd1 * filtered_error_derivative1; + error_prev1 = theta_error; + + // Sum all parts and return it + float torque1 = torque_p1 + torque_i1 + torque_d1; + return torque1; } - static float error_integral = 0; - static float error_prev = 0; - static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); - - // Proportional part: - float torque_p = Kp * theta_error; - - // Integral part: - error_integral = error_integral + theta_error * Ts; - float torque_i = Ki * error_integral; - - // Derivative part: - float error_derivative = (theta_error - error_prev)/Ts; - float filtered_error_derivative = LowPassFilter.step(error_derivative); - float torque_d = Kd * filtered_error_derivative; - error_prev = theta_error; - - // Sum all parts and return it - float torque = torque_p + torque_i + torque_d; - return torque; } void Kinematics() @@ -247,7 +272,7 @@ { //Kinematics(); direction_motor_1 = Controller(CalculateError(theta_ref1,theta_1),0) <= 0.0f; - direction_motor_2 = Controller(CalculateError(theta_ref2,theta_2),1) <= 0.0f; + direction_motor_2 = Controller(CalculateError(theta_ref2,theta_2),1) >= 0.0f; } void ReadEncoder() @@ -260,8 +285,8 @@ { ReadEncoder(); CalculateDirectionMotor(); - PWM_motor_1.write(fabs(torque_1)/(2.0f*pi)); - PWM_motor_2.write(fabs(torque_2)/(2.0f*pi)); + PWM_motor_1.write(fabs(Controller(CalculateError(theta_ref1,theta_1),0))/(2.0f*pi)); + PWM_motor_2.write(fabs(Controller(CalculateError(theta_ref2,theta_2),1))/(2.0f*pi)); } void go_next_1() @@ -430,12 +455,12 @@ void WriteScope() { emgsample(); - scope.set(0, theta_ref1); - scope.set(1, theta_1); - scope.set(2, theta_ref2); - scope.set(3, theta_2); - scope.set(4, CalculateError(theta_ref1,theta_1)); - scope.set(5, CalculateError(theta_ref2,theta_2)); + scope.set(3, theta_ref1); + scope.set(4, theta_1); + scope.set(5, CalculateError(theta_ref1,theta_1)); + scope.set(0, theta_ref2); + scope.set(1, theta_2); + scope.set(2, CalculateError(theta_ref2,theta_2)); scope.send(); } @@ -576,7 +601,8 @@ void while_vertical_movement() { // Do vertical movement stuff - //theta_ref1 = *(1.0f+sin(2*pi*sinus_time.read())); + //theta_ref1 = 0.35f*(0.8f+sin(2*pi*sinus_time.read()*0.1f)); + theta_ref1 = 0.35f; MotorControl(); if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken CurrentState = horizontal_movement; @@ -588,6 +614,9 @@ void while_horizontal_movement() { // Do horizontal movement stuff + theta_ref1 = 0.3f*(0.8f+sin(2*pi*sinus_time.read()*0.1f)); + theta_ref2 = 0.3f*(1.0f+sin(2*pi*sinus_time.read()*0.1f)); + MotorControl(); if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken CurrentState = vertical_movement; StateChanged = true; @@ -667,7 +696,7 @@ PWM_motor_2.period_ms(1.0f/16.0f); //motor_control.attach(&MotorControl, Ts); write_scope.attach(&WriteScope, 0.01); - TickerStateMachine.attach(&StateMachine,1.0f/60.0f); + TickerStateMachine.attach(&StateMachine,Ts); while(true) { return 0; }