Diff: main.cpp

Revision:

30:a16519224d58

Parent:

29:5a846abba59e

Child:

31:465a6e1e1db6

diff -r 5a846abba59e -r a16519224d58 main.cpp
--- 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) {