Diff: main.cpp

Revision:

34:89a424fd37ce

Parent:

33:1da600f06862

Child:

35:ab9b1c9b6d08

diff -r 1da600f06862 -r 89a424fd37ce main.cpp
--- a/main.cpp	Thu Oct 31 10:05:31 2019 +0000
+++ b/main.cpp	Thu Oct 31 13:06:38 2019 +0000
@@ -184,47 +184,49 @@
     }
 }
 
-void Controller()
+float CalculateError(float theta_reference,float theta)
+{
+    float theta_error = theta_reference-theta;
+    return theta_error;
+}
+
+float Controller(float theta_error, bool motor)
 {
-    float K     = 1;
-    float ti    = 0.1;
-    float td    = 10;
-    Kp    = K*(1+td/ti);
-    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;
-    float error_prev2 = 0;
+    if (motor == false) {
+        float K     = 1;
+        float ti    = 0.1;
+        float td    = 10;
+        Kp    = K*(1+td/ti);
+        Ki    = K/ti;
+        Kd    = K*td;
+    } else {
+        float K     = 1;
+        float ti    = 0.1;
+        float td    = 10;
+        Kp    = K*(1+td/ti);
+        Ki    = K/ti;
+        Kd    = K*td;
+    }
+    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_p1 = Kp * theta_error1;
-    float torque_p2 = Kp * theta_error2;
-    
+    float torque_p = Kp * theta_error;
+
     // Integral part:
-    error_integral1 = error_integral1 + theta_error1 * Ts;
-    error_integral2 = error_integral2 + theta_error2 * Ts;
-    float torque_i1 = Ki * error_integral1;
-    float torque_i2 = Ki * error_integral2;
+    error_integral = error_integral + theta_error * Ts;
+    float torque_i = Ki * error_integral;
 
     // Derivative part:
-    float error_derivative1 = (theta_error1 - error_prev1)/Ts;
-    float error_derivative2 = (theta_error2 - error_prev2)/Ts;
-    float filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
-    float filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
-    float torque_d1 = Kd * filtered_error_derivative1;
-    float torque_d2 = Kd * filtered_error_derivative2;
-    error_prev1 = theta_error1;
-    error_prev2 = theta_error2;
+    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
-    torque_1 = torque_p1 + torque_i1 + torque_d1;
-    torque_2 = torque_p2 + torque_i2 + torque_d2;
+    float torque = torque_p + torque_i + torque_d;
+    return torque;
 }
 
 void Kinematics()
@@ -243,10 +245,9 @@
 }
 void CalculateDirectionMotor()
 {
-    Kinematics();
-    Controller();
-    direction_motor_1 = torque_1 <= 0.0f;
-    direction_motor_2 = torque_2 <= 0.0f;
+    //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;
 }
 
 void ReadEncoder()
@@ -429,12 +430,12 @@
 void WriteScope()
 {
     emgsample();
-    scope.set(0, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
-    scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br));
-    scope.set(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg));
-    scope.set(3, emgFiltered_bl);
-    scope.set(4, emgFiltered_br);
-    scope.set(5, emgFiltered_leg);
+    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.send();
 }
 
@@ -574,39 +575,11 @@
 
 void while_vertical_movement()
 {
-    /*
     // Do vertical movement stuff
-    if (emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) {
-        EMGy_velocity = -0.02f;
-    }
-    else if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br))) {
-        EMGy_velocity = 0.02f;
-    }    
-    else {
-        EMGy_velocity = 0.0f;
-    }
+    //theta_ref1 = *(1.0f+sin(2*pi*sinus_time.read()));
+    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;
-        StateChanged = true;
-    }*/
-      m++;
-    if (m<5) {
-        EMGy_velocity = -0.02f;
-        pc.printf("beweging %f \n\r", EMGy_velocity);
-        
-    }
-    else if (m>=5 && m<=10) {
-        EMGy_velocity = 0.02f;
-        pc.printf("beweging %f \n\r", EMGy_velocity);
-    }    
-    
-    else {
-        EMGy_velocity = 0.0f;
-        //pc.printf("beweging %f \n\r", EMGy_velocity);
-    }
-
-    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;
         StateChanged = true;  
     }
 }
@@ -615,42 +588,10 @@
 void while_horizontal_movement()
 {
     // Do horizontal movement stuff
-    
-    if ((emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) || (get_command_a())){
-        EMGx_velocity = -0.02f;
-        pc.printf("beweging %f \n\r", EMGx_velocity);
-    }
-    if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)) || (get_command_d())) {
-        EMGx_velocity = 0.02f;
-        pc.printf("beweging %f \n\r", EMGx_velocity);
-    }    
-    if ((!(emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) && !(emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)))) && (!get_command_a() && !get_command_d())) {
-        EMGx_velocity = 0.0f;
-        pc.printf("beweging %f \n\r", EMGx_velocity);
-    }
-    if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg))) || (get_command_s())) { // EMG gebaseerde threshold aanmaken
+    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;
     }
-    
-    /*
-    if (beweging == 'a') {
-        EMGx_velocity = -0.02f;
-        pc.printf(" you pressed %c \n\r" , beweging);
-    
-    }
-    if (beweging == 'd') {
-        EMGx_velocity = 0.02f;
-        pc.printf(" you pressed %c \n\r" , beweging);
-    }    
-    else {
-        EMGx_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
-        CurrentState = vertical_movement;
-        StateChanged = true;
-    }*/
 }    
 
 
@@ -721,12 +662,13 @@
     ledoff();
     button_1.fall(go_next_1);
     button_2.fall(go_next_2);
-    //sinus_time.start();
-    PWM_motor_1.period_ms(10);
-    PWM_motor_2.period_ms(10);
-    motor_control.attach(&MotorControl, Ts);
+    sinus_time.start();
+    PWM_motor_1.period_ms(1.0f/16.0f);
+    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);
     while(true) {
-        StateMachine();
+        return 0;
     }
 }