Diff: main.cpp

Revision:

33:976be2825a23

Parent:

32:56a8bd82e971

Child:

34:b8b18ba0c336

--- a/main.cpp	Thu Nov 01 18:58:28 2018 +0000
+++ b/main.cpp	Fri Nov 02 11:57:51 2018 +0000
@@ -185,7 +185,7 @@
                 wait(0.001f);                           //Does there need to be a wait?
             }
             Mean0       = sum/sizeCal;                  //Calculate mean of the datapoints in the calibration set (2000 samples)
-            Threshold0  = Mean0*0.8;                    //Threshold calculation calve = 0.8*mean                                         
+            Threshold0  = Mean0*0.5;                    //Threshold calculation calve = 0.8*mean                                         
             break;                                      //Stop. Threshold is calculated, we will use this further in the code
         }
         case 1:                                         //If calibration state 1:
@@ -348,14 +348,13 @@
       // Sum all parts and return it
       u2 = u_k2 + u_i2 + u_d2;  
 }
-
 void engine_control1()                                           //Engine 1 is translational engine, connected with left side pins
 {
             //pc.printf("ik doe het, engine control 1\n\r");
         encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
             //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses());
              //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1);
-        err1 = q1ref - encoder_radians1;
+        err1 = q1_motor - encoder_radians1;
             //pc.printf("err1 = %f\n\r",err1);
         PID_control1();                               //PID controller function call
         
@@ -377,7 +376,7 @@
         encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
             //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses());
             //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2);
-        err2 = q2ref - encoder_radians2;
+        err2 = q2_motor - encoder_radians2;
             //pc.printf("err2 = %f\n\r",err2);
         PID_control2();                            //PID controller function call
              //pc.printf("u2 = %f\n\r",u2);
@@ -393,17 +392,55 @@
         // }
 }
 
+
+/*void engine_control1()                                           //Engine 1 is translational engine, connected with left side pins
+{
+        encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
+        err1 = q1_motor - encoder_radians1;
+        PID_control1();                               //PID controller function call
+               
+        if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1)                              //limits translation in counts, eerst 12600
+            {
+                 pwmpin1 = fabs(u1);                                         
+                 directionpin1.write(u1<0);
+            }
+        else
+            {
+             pwmpin1 = fabs(u1);                                         
+             directionpin1.write(u1>0);   
+            } 
+}
+
+void engine_control2()                                             //Engine 2 is rotational engine, connected with right side wires
+{
+        encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
+        err2 = q2_motor - encoder_radians2;
+        PID_control2();                            //PID controller function call
+            
+        if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250)                              //limits rotation, in counts                
+            {
+                pwmpin2 = fabs(u2);                                       //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+                directionpin2.write(u2>0);
+            }
+            else
+            {
+                pwmpin2 = fabs(u2);                                       //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+                directionpin2.write(u2<0);
+            }
+}
+*/
+
 //------------------ Inversed Kinematics --------------------------------//
 
 void inverse_kinematics()
 {    
-    //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
+       //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
     
     //Lq1 = q1ref*r_trans;                            
     //Cq2 = q2ref/5.0;                               
 
     q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref);               //RKI systeem  
-    q2_dot = v_y/(L3*cos(q2ref));                                          // Misschien gain toevoegen om te kijken of het dan werkt. Translatie gaat veel trager en moeizamer dan rotatie
+    q2_dot = v_y/(L3*cos(q2ref));                                          //
 
     q1_ii = q1ref + q1_dot*T;                         //Omgezet naar motorhoeken
     q2_ii = q2ref + q2_dot*T; 
@@ -411,16 +448,12 @@
     q1ref = q1_ii;
     q2ref = q2_ii;
     
-    q1_motor = q1ref*5.0;
-    q2_motor = q2ref/r_trans;  
-    
+    q1_motor = -q1ref/r_trans;
+    q2_motor = q2ref*5.0;      
     
-    //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
-
-    
-    //start_control = 1;
     engine_control1();                           
     engine_control2();
+
 }
 
 void v_des_calculate_qref()
@@ -429,7 +462,7 @@
     { 
                 if(movAg1>Threshold1 && movAg0<Threshold0)              //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0)
                 {
-                    v_x = 0.5;                                          //movement in +x direction
+                    v_x = 0.05;                                          //movement in +x direction
                     v_y = 0.0;
                     
                     ledr = 0;                                           //red
@@ -438,7 +471,7 @@
                 }
                 else if(movAg2>Threshold2 && movAg0<Threshold0)         //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0)
                 {
-                    v_y = 0.5;                                          //Movement in +y direction
+                    v_y = 0.05;                                          //Movement in +y direction
                     v_x = 0.0;
                     
                     ledr = 1;                                           //Green
@@ -449,7 +482,7 @@
                 else if(movAg0>Threshold0 && movAg1>Threshold1)         //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0)
                 {
                     v_y = 0.0;                                          //Movement in -x direction
-                    v_x = -0.5;
+                    v_x = -0.05;
                     
                     ledr = 0;                                           //Purple
                     ledb = 0;
@@ -458,7 +491,7 @@
                 
                 else if(movAg0>Threshold0 && movAg2>Threshold2)         //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0)
                 {
-                    v_y = -0.5;                                         //Movement in -y direction
+                    v_y = -0.05;                                         //Movement in -y direction
                     v_x = 0.0;
                     
                     ledr = 1;                                           //Blue