Diff: main_demo.cpp

Revision:

4:98b3dfdd9ae1

Parent:

3:fd1380ffb200

Child:

5:cd329205f037

--- a/main_demo.cpp	Wed Oct 31 17:06:04 2018 +0000
+++ b/main_demo.cpp	Wed Oct 31 18:43:28 2018 +0000
@@ -12,7 +12,7 @@
 DigitalOut directionM2(D7);
 FastPWM motor1_pwm(D5);
 FastPWM motor2_pwm(D6);
-MODSERIAL  pc(USBTX, USBRX);
+MODSERIAL pc(USBTX, USBRX);
 DigitalOut ledr(LED_RED);
 DigitalOut ledg(LED_GREEN);
 DigitalOut ledb(LED_BLUE);
@@ -98,7 +98,7 @@
     if ((setpointy > point2y) && (track == 12)) {
         setpointx = point2x;
         //setpointx = setpointx + 0.2;
-        setpointy = setpointy + (0.2);
+        setpointy = setpointy + (-0.2);
     }
     
     if ((fabs(setpointx - point2x) <= 0.3) && (fabs(setpointy - point2y) <= 0.3) && (track == 12)) {
@@ -193,6 +193,7 @@
     // Return
     return U2;      
 } 
+
 // ------------------------------------------------------------
 // ------------ Inverse Kinematics ----------------------------
 // ------------------------------------------------------------
@@ -231,7 +232,7 @@
     motor1_pwm.write(fabs(U1));         // Motor aansturen
     directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
     motor2_pwm.write(fabs(U2));
-    directionM2 = U2 > 0.0f; 
+    directionM2 = U2 > 0.0f;     
 }
 
 
@@ -255,7 +256,7 @@
     pc.printf("\r\n\r\nDOE HET AUB!!! \r\n\r\n");
     
     while (true) {
-        pc.printf("Setpointx: %0.2f en Setpointy: %02f \r\n", setpointx,setpointy);
+        pc.printf("Setpointx: %0.2f, Setpointy: %02f, q1_diff: %0.2f, q2_diff: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff, U1,U2);
         if (track == 1) {
             pc.printf("Gaat naar positie 1\r\n");
         }
@@ -263,5 +264,6 @@
             pc.printf("Gaat naar positie 2\r\n");
         }
         wait(0.5f);
+   
     }
 }    
\ No newline at end of file