Diff: main.cpp

Revision:

29:5a846abba59e

Parent:

28:8c90a46b613e

Child:

30:a16519224d58

--- a/main.cpp	Tue Oct 29 17:14:23 2019 +0000
+++ b/main.cpp	Wed Oct 30 11:05:31 2019 +0000
@@ -49,26 +49,27 @@
 volatile bool    pressed_2    = false;
 HIDScope scope(6);
 
-//volatile float theta_1;
 //volatile float  theta_error_1;
-volatile float  theta_reference_1;
-//volatile float theta_2;
+volatile float  theta_ref1;
 //volatile float  theta_error_2;
-volatile float  theta_reference_2;
+volatile float  theta_ref2;
 float Ts    = 0.01;
 float Kp;
 float Ki;
 float Kd;
-
-float theta;
 float thetav_1;
 float thetav_2;
-float theta_1 = (90.0f*pi)/180.0f;
-float theta_2 = (90.0f*pi)/180.0f;
+float theta_1 = (40.0f*pi)/180.0f;
+float theta_2 = (175.0f*pi)/180.0f;
+float theta_error1;
+float theta_error2;
+float torque_1;
+float torque_2;
 float x;
 float y;
 volatile float EMGx_velocity=0.02;
 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);
@@ -157,49 +158,51 @@
 }
 
 
-float CalculateError(float theta_reference,float theta)
+void CalculateError()
 {
-    float theta_error = theta_reference-theta;
-    return theta_error;
+    theta_error1 = theta_ref1-theta_1;
+    theta_error2 = theta_ref2-theta_2;
 }
 
-float Controller(float theta_error, bool motor)
+void Controller()
 {
-    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;
+    float K     = 1;
+    float ti    = 0.1;
+    float td    = 10;
+    Kp    = K*(1+td/ti);
+    Ki    = K/ti;
+    Kd    = K*td;
+    
+    float error_integral1 = 0;
+    float error_integral2 = 0;
+    float error_prev1 = 0;
+    float error_prev2 = 0;
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
 
     // Proportional part:
-    float torque_p = Kp * theta_error;
-
+    float torque_p1 = Kp * theta_error1;
+    float torque_p2 = Kp * theta_error2;
+    
     // Integral part:
-    error_integral = error_integral + theta_error * Ts;
-    float torque_i = Ki * error_integral;
+    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;
 
     // 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;
+    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;
 
     // Sum all parts and return it
-    float torque = torque_p + torque_i + torque_d;
-    return torque;
+    torque_1 = torque_p1 + torque_i1 + torque_d1;
+    torque_2 = torque_p2 + torque_i2 + torque_d2;
+
 }
 
 void AngleVelocity()
@@ -211,11 +214,11 @@
 
 void JointAngle()
 {
-    float theta_ref1=theta_1+thetav_1*Ts;
-    float theta_ref2=theta_2+thetav_2*Ts;
+    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) {
+    if (sqrt(pow(x,2)+pow(y,2))>1.0f) {
         theta_1=theta_ref1;
         theta_2=theta_ref2;
     }
@@ -223,8 +226,9 @@
 
 void CalculateDirectionMotor()
 {
-    direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f;
-    direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f;
+    Controller();
+    direction_motor_1 = torque_1 <= 0.0f;
+    direction_motor_2 = torque_2 <= 0.0f;
 }
 
 void ReadEncoder()
@@ -235,11 +239,11 @@
 
 void MotorControl()
 {
+    Controller();
     ReadEncoder();
-    theta_reference_1 = 360.0f*sin(0.1f*sinus_time.read()*2.0f*3.14f); // voor test, moet weg in eindscript
     CalculateDirectionMotor();
-    PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f));
-    PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f));
+    PWM_motor_1.write(fabs(torque_1)/360.0f);
+    PWM_motor_2.write(fabs(torque_2)/360.0f);
 }
 
 void go_next_1()
@@ -460,7 +464,9 @@
 
 void while_motor_calibration()
 {
+    
     // Do motor calibration stuff
+    
     if (pressed_1) { // bool aanmaken voor demo (switch oid aanmaken)
         CurrentState = demo_mode;
         StateChanged = true;
@@ -548,6 +554,7 @@
 
 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;
@@ -561,12 +568,26 @@
     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;
+    }*/
+    if (beweging =='s') {
+        EMGy_velocity = -0.02f;
     }
+    else if (beweging == 'w') {
+        EMGy_velocity = 0.02f;
+    }    
+    else {
+        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
+        CurrentState = horizontal_movement;
+        StateChanged = true;  
+        }
 }
 
 void while_horizontal_movement()
 {
     // Do horizontal movement stuff
+    /*
     if (emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) {
         EMGx_velocity = -0.02f;
     }
@@ -580,7 +601,23 @@
         CurrentState = vertical_movement;
         StateChanged = true;
     }
-}
+    */
+    if (beweging == 'a') {
+        EMGx_velocity = -0.02f;
+        pc.printf(" you pressed %c \n\r" , beweging);
+    }
+    else if (beweging == 'd') {
+        EMGx_velocity = 0.02f;
+    }    
+    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;
+    }
+}    
+
 
 void StateTransition()
 {
@@ -650,10 +687,12 @@
     button_1.fall(go_next_1);
     button_2.fall(go_next_2);
     //sinus_time.start();
-    //PWM_motor_1.period_ms(10);
-    //motor_control.attach(&MotorControl, Ts);
+    PWM_motor_1.period_ms(10);
+    PWM_motor_2.period_ms(10);
+    motor_control.attach(&MotorControl, Ts);
     write_scope.attach(&WriteScope, 0.01);
-    //TickerStateMachine.attach(StateMachine,1.00f);
+    TickerStateMachine.attach(StateMachine,1.00f);
+    beweging = pc.getc();
     while(true) {
         StateMachine();
     }