Diff: main.cpp

Revision:

5:205f1de452de

Parent:

4:a97c2e8a80d8

Child:

6:0256de2854d6

--- a/main.cpp	Tue Oct 30 12:18:08 2018 +0000
+++ b/main.cpp	Tue Oct 30 14:02:01 2018 +0000
@@ -5,25 +5,22 @@
 // Define the HIDScope and Ticker object
 HIDScope    scope(2);
 Ticker      scopeTimer;
-Ticker      ikTimer;
-
+Ticker      IKticker;
 
 // Constantes
 const double L1 = 0.35;
 const double L2 = 0.30;
-const double T  = 3;
+double T;
 
-//Variables
-double q1 = 0;
-double q2 = 90;
-double vx = 0.05;
-double vy = 0;
+double q1;
+double q2;
+
 
 //Hoe werken de variabls?
 // q1 en q2 komen uit de Encoder dus die worden geleverd
 // This is the function for the Inverse Kinematics We start with a inverse Jacobian so we can determine q_dot (rotation speed of the motors)
 
-double IK() {
+double IK(double q1, double q2, double vx, double vy) {
     
     double invj[2][2] =
     {
@@ -31,8 +28,8 @@
         {-(L2*sin(q1 + q2) + L1*sin(q1))/(L1*L2*cos(q1 + q2)*sin(q1) - L1*L2*sin(q1 + q2)*cos(q1)), (L2*cos(q1 + q2) + L1*cos(q1))/(L1*L2*cos(q1 + q2)*sin(q1) - L1*L2*sin(q1 + q2)*cos(q1))}
     };
     
-    double V_q1 = invj[1][1]*vx + invj[1][2]*vy;
-    double V_q2 = invj[2][1]*vx + invj[2][2]*vy;
+    double V_q1 = invj[0][0]*vx + invj[0][1]*vy;
+    double V_q2 = invj[1][0]*vx + invj[1][1]*vy;
     
     // Numerical Integral to make it position controlled
     double q1_new = q1 + V_q1*T;
@@ -40,25 +37,27 @@
     double q2_new = q2 + V_q2*T;
     q2 = q2_new;
     
-    return(q1, q2);
-    
 }
 
 // Write a function where the data gets send to HIDScope
 void scopeSend()
 {
-    scope.set(0,q1);
-    scope.set(1,q2);
-    scope.send();
+  
+     scope.set(0,q1);
+     scope.set(1,q2);
+     scope.send();
 }
 
     
 int main()
 {
-    // Attach the data read and send function at 0.1 s
-    ikTimer.attach_us(&IK, 0.1);
-    // Attach the data read and send function at 100 Hz
-    scopeTimer.attach_us(&scopeSend, 2e3);   
+    while(T<3) {
+        T = T + 0.1;
+            IK(R1, R2, 0.05, 0);
+            IKticker.attach(&IK);
+            // Attach the data read and send function at 100 Hz
+            scopeTimer.attach_us(&scopeSend);
+    } 
     
-    while(1) { }
+    
 }
\ No newline at end of file