Diff: main.cpp

Revision:

35:ab9b1c9b6d08

Parent:

34:89a424fd37ce

Child:

36:66f500e387c4

--- 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;
     }