Revision 36:66f500e387c4, committed 2019-10-31

Comitter:

sembert

Date:

Thu Oct 31 21:49:14 2019 +0000

Parent:

35:ab9b1c9b6d08

Commit message:

lololol;

Changed in this revision

diff -r ab9b1c9b6d08 -r 66f500e387c4 main.cpp
--- a/main.cpp	Thu Oct 31 17:44:11 2019 +0000
+++ b/main.cpp	Thu Oct 31 21:49:14 2019 +0000
@@ -32,8 +32,8 @@
 // variables
 int m = 0;
 
-const float pi = 3.1416;
-const float l = 0.535;
+const float pi = 3.1416f;
+const float l = 0.535f;
 Ticker          TickerStateMachine;
 Ticker          motor_control;
 Ticker          write_scope;
@@ -51,17 +51,17 @@
 volatile bool    pressed_2    = false;
 HIDScope scope(6);
 
-volatile float  theta_ref1;
-volatile float  theta_ref2;
-float Ts    = 0.01f;
+volatile float  theta_ref1 = 0.5f*pi;
+volatile float  theta_ref2 = 3.0f*pi/4.0f;
+float Ts    = 0.1f;
 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_1;
+float theta_2;
 float theta_error1;
 float theta_error2;
 float torque_1;
@@ -107,6 +107,7 @@
 bool current_value_emg_leg;
 
 volatile char command;
+
 // functies
 void ledred()
 {
@@ -255,30 +256,32 @@
 }
 
 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)));  
-    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;  
+    {    
+    EMGx_velocity = 0.1f;  
+    EMGy_velocity = 0.0f; 
+    float thetav_1= -(EMGx_velocity*cos(theta_1 + theta_2))/(l*cos(theta_1 + theta_2)*sin(theta_1) - l*sin(theta_1 + theta_2)*cos(theta_1)) - (EMGy_velocity*sin(theta_1 + theta_2))/(l*cos(theta_1 + theta_2)*sin(theta_1) - l*sin(theta_1 + theta_2)*cos(theta_1));
+    float thetav_2= (EMGx_velocity*(cos(theta_1 + theta_2) + cos(theta_1)))/(l*cos(theta_1 + theta_2)*sin(theta_1) - l*sin(theta_1 + theta_2)*cos(theta_1)) + (EMGy_velocity*(sin(theta_1 + theta_2) + sin(theta_1)))/(l*cos(theta_1 + theta_2)*sin(theta_1) - l*sin(theta_1 + theta_2)*cos(theta_1));
+    theta_ref1 = theta_ref1 + thetav_1*Ts;
+    theta_ref2 = theta_ref2 + thetav_2*Ts;
+    x = cos(theta_ref1)*l+cos(theta_ref1+theta_ref2)*l;
+    y = l*(sin(theta_ref1)+sin(theta_ref1+theta_ref2));  
     if (sqrt(pow(x,2)+pow(y,2))>1.0f) {
+        pc.printf("Limit reached!\n\r");
         theta_ref1 = theta_1;
         theta_ref2 = theta_2;
     }
 }
 void CalculateDirectionMotor()
 {
-    //Kinematics();
-    direction_motor_1 = Controller(CalculateError(theta_ref1,theta_1),0) <= 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()
 {
-    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;
+    theta_1 = (0.5f*pi-(((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 = ((-3.0f*pi/4.0f)+(((2.0f*pi)/64.0f)*(float)encoder_2.getPulses())/131.25f);
 }
 
 void MotorControl()
@@ -455,12 +458,12 @@
 void WriteScope()
 {
     emgsample();
-    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.set(0, theta_ref1);
+    scope.set(1, theta_1);
+    scope.set(2, CalculateError(theta_ref1,theta_1));
+    scope.set(3, theta_ref2);
+    scope.set(4, theta_2);
+    scope.set(5, CalculateError(theta_ref2,theta_2));
     scope.send();
 }
 
@@ -507,9 +510,7 @@
 
 void while_motor_calibration()
 {
-    
     // Do motor calibration stuff
-    
     if (pressed_1) { // bool aanmaken voor demo (switch oid aanmaken)
         CurrentState = demo_mode;
         StateChanged = true;
@@ -522,14 +523,47 @@
 
 void while_demo_mode()
 {
-    // Do demo mode stuff
-    treshold_bl = 1.1f;
-    treshold_br = 1.1f;
-    treshold_leg = 1.1f;
-    if ((pressed_1) || (pressed_2)) {
-        CurrentState = vertical_movement;
+    EMGx_velocity = 0.1f;
+    EMGy_velocity = 0.0f;
+    ReadEncoder();
+    Kinematics();
+    
+    pc.printf("X: %f    Y: %f   1: %f    2: %f  \n\r",x,y,theta_1,theta_2);
+    /*
+    m++;
+    
+    if (m<5) {
+        EMGy_velocity = 0.1f;
+        EMGx_velocity = 0.0f;
+        pc.printf("beweging in y richting %f \n\r", EMGy_velocity);
+        pc.printf("thetav_1 %f \n\r", theta_ref1);
+    }
+    if (m>=5 && m<10) {
+        EMGy_velocity = 0.0f;
+        EMGx_velocity = 0.1f;
+        pc.printf("beweging in x richting %f \n\r", EMGx_velocity);
+        pc.printf("thetav_1 %f \n\r", theta_ref1);
+    }
+    if (m>=10 && m<15) {
+        EMGy_velocity = -0.1f;
+        EMGx_velocity = 0.0f;
+        pc.printf("beweging in y richting %f \n\r", EMGy_velocity);
+    }
+    if (m>=15 && m<20) {
+        EMGy_velocity = 0.0f;
+        EMGx_velocity = -0.1f;
+        pc.printf("beweging in x richting %f \n\r", EMGx_velocity);
+    }
+    if (pressed_1) {
+        m=0;
+        CurrentState = demo_mode;
         StateChanged = true;
     }
+    if ((pressed_2)) {
+        CurrentState = emg_calibration;
+        StateChanged = true;
+    }
+    */
 }
 
 void while_emg_calibration()
@@ -601,8 +635,6 @@
 void while_vertical_movement()
 {
     // Do vertical movement stuff
-    //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;
@@ -614,8 +646,6 @@
 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;