Diff: main.cpp

Revision:

4:a97c2e8a80d8

Parent:

3:2a437a6ef149

Child:

5:205f1de452de

--- a/main.cpp	Tue Oct 30 10:56:28 2018 +0000
+++ b/main.cpp	Tue Oct 30 12:18:08 2018 +0000
@@ -5,7 +5,7 @@
 // Define the HIDScope and Ticker object
 HIDScope    scope(2);
 Ticker      scopeTimer;
-Ticker      IKTicker;
+Ticker      ikTimer;
 
 
 // Constantes
@@ -14,17 +14,16 @@
 const double T  = 3;
 
 //Variables
-double q1;
-double q2;
-double vx;
-double vy;
-
+double q1 = 0;
+double q2 = 90;
+double vx = 0.05;
+double vy = 0;
 
 //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)
 
-void Inverse_Kinetmatics(&q1 &q2 &q1_new &q2_new) {
+double IK() {
     
     double invj[2][2] =
     {
@@ -40,21 +39,24 @@
     q1 = q1_new;
     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.set(0,q1);
+    scope.set(1,q2);
     scope.send();
 }
 
     
 int main()
 {
-    // Attach the data read and send function at 100 Hz
-    IKTicker.attach_us(&Inverse_Kinematics, 0.1);
+    // 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);