Diff: main.cpp

Revision:

9:d5c561b3ea5a

Parent:

8:1efebfebe733

Child:

10:4034134fd7db

--- a/main.cpp	Tue Oct 30 12:17:35 2018 +0000
+++ b/main.cpp	Tue Oct 30 16:57:53 2018 +0000
@@ -15,32 +15,46 @@
  
 DigitalOut motor2direction(D4);
 PwmOut motor2control(D5);
+
+InterruptIn button1(D10);
+
+Serial pc(USBTX, USBRX);
  
-void EncoderFunc() {
-    
+ 
+ double translatie;
+ double hoekgraad;
+ double hoekgraad2;
+ 
+     
     const float pi = 3.141592653589793;                             // Value of pi
     double gearratio = 3.857142857;
     double radiuspulley = 0.015915;                                 // Radius pulley [m]
-    double hoekgraad = Encoder1.getPulses() * 0.0857142857;         // Angle arm [degree]
-    double hoekrad = hoekgraad * 0.0174532925;
-    double hoekgraad2 = Encoder2.getPulses() * 0.0857142857;
-    double hoekrad2 = hoekgraad2 * 0.0174532925;
-    double hoekarm = hoekgraad2 / gearratio;
-    double translatie = hoekgraad /360 * 2 * pi * radiuspulley;     // Translatie arm [m]
+
+    
+void EncoderFunc() {
+    hoekgraad = Encoder1.getPulses() * 0.0857142857;         // Angle arm [degree]
+   // double hoekrad = hoekgraad * 0.0174532925;
+    hoekgraad2 = Encoder2.getPulses() * 0.0857142857;
+  //  double hoekrad2 = hoekgraad2 * 0.0174532925;
+  //  double hoekarm = hoekgraad2 / gearratio;
+    translatie = hoekgraad /360.0 * 2.0 * pi * radiuspulley;     // Translatie arm [m]
  }
 
 //----------------INVERSE KINEMATICS ---------------------------
-double K_v = 1;                                     // Velocity constant (VALUE???) Maximaal 6.6667
-double L0 = 0.09;
+double K_v = 1.00; //velocity constant, max 6.667
+double L0 = 0.09; // starting length
 
 int main()
 {   
-EncoderTicker.attach(&EncoderFunc, 1);
+EncoderTicker.attach(&EncoderFunc, 0.005);
+pc.baud(115200);
+pc.printf("translatie:%f /n hoekgraad:%f /n hoekgraad2:%f", translatie, hoekgraad, hoekgraad2);
+
 
-double p_old_x = (translatie+L0)*cos(hoekgraad);          // Everytime the x-value from encoder calculated
-double p_old_y = (translatie+L0)*sin(hoekgraad);          // Everytime the y-value from encoder calculated
+double p_old_x = (translatie+L0)*cos(hoekgraad2);          // Everytime the x-value from encoder calculated
+double p_old_y = (translatie+L0)*sin(hoekgraad2);          // Everytime the y-value from encoder calculated
 
-double J_inv_1_1 = -sin(hoekgraad)/translatie;
+double J_inv_1_1 = -sin(hoekgraad)/translatie;            // Construction of inverse Jacobian
 double J_inv_1_2 = cos(hoekgraad)/translatie;
 double J_inv_2_1 = cos(hoekgraad);
 double J_inv_2_2 = sin(hoekgraad);
@@ -62,31 +76,42 @@
     y_path[4] = y_path[0];
 
 // for loop
+
+
 for(int i=0 ; i<=4 ; i++)
     {
     double p_new_x = x_path[i];
     double p_new_y = y_path[i];
     
-    double p_dot_new_x = K_v * (p_new_x - p_old_x);
+    double p_dot_new_x = K_v * (p_new_x - p_old_x); //Snelheid is constante K_V * distance(p_old p_new)
     double p_dot_new_y = K_v * (p_new_y - p_old_y);
     
    // printf("x=%f , y=%f , p_dot_new_x=%f , p_dot_new_y=%f\n",p_new_x,p_new_y,p_dot_new_x,p_dot_new_y);
     
-    double angle_old = atan(p_old_y/p_old_x)*180/pi;
+    double angle_old = atan(p_old_y/p_old_x)*180/pi; //Dynamische manier om hoek en lengte verandering te bepalen
     double L_old = sqrt(pow(p_old_x,2)+pow(p_old_y,2));
     
     double angle_new = atan(p_new_y/p_new_x)*180/pi;
     double L_new = sqrt(pow(p_new_x,2)+pow(p_new_y,2));
     
-    if (angle_new - angle_old) <= 0 
-        {
-        motor1direction.write(direction1 = !direction1); 
-        }       
     
-      if (L_new - L_old) <= 0 
+    if (angle_new - angle_old <= 0) // als hoekveranding ccw > motor cw > true
         {
-        motor2direction.write(direction2 = !direction2);        
-        } 
+        motor2direction = true;
+        }
+        else
+            {
+            motor2direction = false;
+            }      
+    
+      if (L_new - L_old <= 0 )// als lengteverandering negatief > to base (ccw) > true
+        {
+        motor1direction = true;
+        }
+        else
+            {
+            motor1direction = false;
+            }
     
         while ( (fabs(p_new_x - p_old_x)) > 0.005 || (fabs(p_new_y - p_old_y)) > 0.005 )
         {