Diff: main.cpp

Revision:

10:4034134fd7db

Parent:

9:d5c561b3ea5a

Child:

11:01372da5a144

--- a/main.cpp	Tue Oct 30 16:57:53 2018 +0000
+++ b/main.cpp	Wed Oct 31 08:18:51 2018 +0000
@@ -1,11 +1,11 @@
-#include "mbed.h"
+ #include "mbed.h"
 #include "math.h"
 #include "BiQuad.h"
 #include "Servo.h"
 #include <string>
 #include "QEI.h"
 
-//-----------------GET ENCODER VALUES -------------------------
+//----------------- INITIAL -------------------------
 QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
 QEI Encoder2(D2,D3,NC,64,QEI::X2_ENCODING);
 Ticker EncoderTicker;
@@ -20,62 +20,64 @@
 
 Serial pc(USBTX, USBRX);
  
- 
- double translatie;
- double hoekgraad;
- double hoekgraad2;
- 
-     
-    const float pi = 3.141592653589793;                             // Value of pi
+    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 radiuspulley = 0.015915;         // Radius pulley [m]
+      
+    double K_v = 1.00;                      //velocity constant, max 6.667 ?
+    double L0 = 0.09;                       // starting length
 
-    
+
+//-----------------GET ENCODER VALUES -------------------------
 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;
+    hoekgraad = Encoder1.getPulses() * 0.0857142857;        
+        // double hoekrad = hoekgraad * 0.0174532925;
+    hoekgraad2 = Encoder2.getPulses() * 0.0857142857;                // Angle arm [degree]
+        //  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.00; //velocity constant, max 6.667
-double L0 = 0.09; // starting length
 
 int main()
 {   
+    motor2direction = false;        // Nu staan motoren toch op het begin allebei in positieve stand?
+    motor2direction = false;
+    
 EncoderTicker.attach(&EncoderFunc, 0.005);
 pc.baud(115200);
-pc.printf("translatie:%f /n hoekgraad:%f /n hoekgraad2:%f", translatie, hoekgraad, hoekgraad2);
+pc.printf("hoekgraad=%f degrees\t translatie:%f meters /t hoekgraad2:%f degrees /n",hoekgraad, translatie, hoekgraad2);
 
-
+//---------------- INVERSE KINEMATICS ---------------------------
 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;            // 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);
+double J_inv_1_1 = -sin(hoekgraad2)/(translatie+L0);       // Construction of inverse Jacobian
+double J_inv_1_2 = cos(hoekgraad2)/(translatie+L0);
+double J_inv_2_1 = cos(hoekgraad2);
+double J_inv_2_2 = sin(hoekgraad2);
 
 
-// Demo path: rectangular
+    // Demo path: rectangular
     double x_path[5];       // Matrix heeft 5 elementen: beginnend vanaf element 0 tot en met element 4
     x_path[0] = L0;
     x_path[1] = L0;
-    x_path[2] = L0+0.15;
-    x_path[3] = L0+0.15;
+    x_path[2] = L0+0.215;
+    x_path[3] = L0+0.215;
     x_path[4] = x_path[0];
 
     double y_path[5];
     y_path[0] = 0.0;
-    y_path[1] = 0.1;
-    y_path[2] = 0.1;
+    y_path[1] = 0.135;
+    y_path[2] = 0.135;
     y_path[3] = 0.0;
     y_path[4] = y_path[0];
 
-// for loop
+    // for loop
 
 
 for(int i=0 ; i<=4 ; i++)
@@ -99,29 +101,37 @@
         {
         motor2direction = true;
         }
-        else
-            {
-            motor2direction = false;
-            }      
+    else
+        {
+        motor2direction = false;
+        }      
     
-      if (L_new - L_old <= 0 )// als lengteverandering negatief > to base (ccw) > true
+    if (L_new - L_old <= 0 )// als lengteverandering negatief > to base (ccw) > true
         {
         motor1direction = true;
         }
-        else
-            {
-            motor1direction = false;
-            }
+    else
+        {
+        motor1direction = false;
+        }
     
-        while ( (fabs(p_new_x - p_old_x)) > 0.005 || (fabs(p_new_y - p_old_y)) > 0.005 )
+        while ( (fabs(p_new_x - p_old_x)) > 0.005 && (fabs(p_new_y - p_old_y)) > 0.005 )
         {
-            double q_dot_angle = J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y;     //hoekgraad1
+            double q_dot_angle = (J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y)*pi/180.0;     //hoekgraad2
             double q_dot_L = J_inv_2_1 * p_dot_new_x + J_inv_2_2 * p_dot_new_y;         //translatie
-            double q_dot_q2 = q_dot_L/radiuspulley;                                     //hoekgraad2 (translatie)
-            motor1control.write(q_dot_angle);
+            double q_dot_q2 = (q_dot_L/radiuspulley) *pi/180.0;                                     //hoekgraad (translatie) in radialen
+            motor1control.write(q_dot_q2);
             wait(0.1);
-            motor2control.write(q_dot_q2);
-            wait(0.1); // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Berekening niet tegelijk, eventuele fout? %%%
-        }
-    }
-}
\ No newline at end of file
+            motor2control.write(q_dot_angle);
+            wait(0.1); // %%%Berekening niet tegelijk, eventuele fout? %%%
+            
+            void setMotor1(float motorValue) {
+            // Given motorValue<=1, writes the velocity to the pwm control.
+            // MotorValues outside range are truncated to within range.
+            motor1control.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue));
+            
+            
+        }       // End of while
+        
+    }           // End of for 
+}               // End of main()
\ No newline at end of file