Diff: main_demo.cpp

Revision:

5:cd329205f037

Parent:

4:98b3dfdd9ae1

Child:

6:5431ddb9d881

--- a/main_demo.cpp	Wed Oct 31 18:43:28 2018 +0000
+++ b/main_demo.cpp	Thu Nov 01 11:03:23 2018 +0000
@@ -3,7 +3,7 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "math.h"
-
+//toch forken
 // --------------------------------------------------
 // ----------------- SET UP -------------------------
 QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, counts/rev)
@@ -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);
@@ -32,12 +32,12 @@
 volatile double  error2;
 double  point1x = 200.0;
 double  point1y = 200.0;
-double  point2x = 200.0;
-double  point2y = 100.0;
-//double  point3x = 350.0;
-//double  point3y = 0.0;
-//double  point4x = 200.0;
-//double  point4y = 0.0;
+double  point2x = 350.0;
+double  point2y = 200.0;
+double  point3x = 350.0;
+double  point3y = 100.0;
+double  point4x = 200.0;
+double  point4y = 100.0;
 volatile int track = 1;
 const double x0 = 80.0; //zero x position after homing
 const double y0 = 141.0; //zero y position after homing
@@ -78,77 +78,111 @@
 // -------------------------------------------------------------------------
 // -------------- Determine Setpoints --------------------------------------
 // -------------------------------------------------------------------------
-void determinedemoset()
+double determinedemosetx(double setpointx, double setpointy)
 {
-    if ((setpointx < point1x) && (track == 1)){ 
-        setpointx = setpointx + 0.2;    
-    }
-    if ((setpointy < point1y) && (track == 1)){
-        setpointy = setpointy + 0.1;
-    } 
-    
-    // Van punt 1 naar punt 2. 
-    if ((setpointy >= point1y - 0.3) && (setpointx >= point1x - 0.3) && (setpointy <= point1y + 0.3) && (setpointx <= point1x + 0.3 && track == 1)){
-        //setpointx = point2x;
-        //setpointy = setpointy + (-0.2);          // Van punt 1 naar punt 2 op dezelfde y blijven. 
-        ledr = !ledr;                                // Aangeven met een ROOD lampje dat hij op de plaats van bestemming is
-        track = 12;
+
+    if (setpointx < point1x && track == 1){ 
+        setpointx = setpointx + 0.1;    
     }
     
-    if ((setpointy > point2y) && (track == 12)) {
-        setpointx = point2x;
-        //setpointx = setpointx + 0.2;
-        setpointy = setpointy + (-0.2);
+    // Van punt 1 naar punt 2. 
+    if (setpointy >= point1y - 0.3 && setpointx >= point1x - 0.3 && setpointy <= point1y + 0.3 && setpointx <= point1x + 0.3 && track == 1){
+        setpointx = setpointx + 0.1;
+        track = 12;
+    }
+    if (setpointx < point2x && track == 12){
+        setpointx = setpointx + 0.2;
     }
     
-    if ((fabs(setpointx - point2x) <= 0.3) && (fabs(setpointy - point2y) <= 0.3) && (track == 12)) {
-        ledr = !ledr;
-        ledg = !ledg;
-        track = 23;
-        }
-    
-    
-    /**
-    if (setpointy > point2y && track == 12){
-        setpointx = point2x;
-        setpointy = setpointy + (-0.2);
-    }
-    
-    
-    // Van punt 2 naar punt 3. 
-    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3)
+    // Van punt 2 naar punt 1. 
+    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3 && track == 12)
     {
-        //setpointx = setpointx - 0.2; 
-        //setpointy = setpointy;
-        ledr = 1;
-        ledg = 0;
+        setpointx = point3x; 
         track = 23;
     }
     if (setpointy > point3y && track == 23)
     {
-        //setpointx = setpointx - 0.2;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
-        //setpointy = setpointy;
-        track = 23;
+        setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
     } 
-    
-        
+ 
+
     // Van punt 3 naar punt 4. 
     if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
     {
         setpointx = setpointx - 0.1;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        track = 34;
+    }
+    
+    if (setpointy > point3y && track == 34)
+    {
+        setpointx = setpointx - 0.1;
+    }
+        
+    if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3 && track == 34)
+    {
+        track = 1;
+    }
+
+      return setpointx;
+}  
+
+double determinedemosety(double setpointx, double setpointy)
+{
+    // Van reference positie naar punt 1.
+    if(setpointy < point1y && track == 1){
+        setpointy = setpointy + (0.2);
+    } 
+
+    // Van punt 1 naar punt 2. 
+    if (fabs(setpointx - point1x) <= 0.3 && fabs(setpointy - point1y) <= 0.3 && track == 1){
+        ledg = 1;
+        ledr = 0;
+        setpointy = point2y;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        track = 12;
+    }
+    if (setpointx < point2x && track == 12){
+        setpointy = point2y;
+    }
+    
+    // Van punt 2 naar punt 3. 
+    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3){
+        ledr = 1;
+        ledg = 0;
+        setpointx = point3x;
+        track = 23;
+    }
+    if ((setpointy > point3y) && (track == 23))
+    {
+        setpointy = setpointy + (-0.2);
+        track = 23;
+    }    
+    
+    // Van punt 3 naar punt 4. 
+    if ((fabs(setpointx - point3x) <= 0.3) && (fabs(setpointy - point3y) <= 0.3) && (track == 23))
+    {
+        ledg = 1;
+        ledr = 1;
+        ledb = 0;
         setpointy = setpointy;
         track = 34;
     }
     if (setpointy > point3y && track == 34)
     {
-        setpointx = setpointx - 0.1;
         setpointy = setpointy;
-
+    }     
+    
+    
+    if ((fabs(setpointx - point4x) <= 0.3) && (fabs(setpointy - point4y) <= 0.3) && (track == 34))
+    {
+        ledg = 1; 
+        ledr = 1; 
+        ledb = 1;
+        track = 1;
     }
-    */
 
-}  
-
+    return setpointy;
+    
+}
 
 // -----------------------------------------------------------------    
 // --------------------------- PI controllers ----------------------
@@ -193,7 +227,6 @@
     // Return
     return U2;      
 } 
-
 // ------------------------------------------------------------
 // ------------ Inverse Kinematics ----------------------------
 // ------------------------------------------------------------
@@ -217,7 +250,8 @@
 // -----------------------------------------------
 void motoraansturing()
 {
-    determinedemoset();
+    setpointx = determinedemosetx(setpointx, setpointy);
+    setpointy = determinedemosety(setpointx, setpointy);
     q1_diff = makeAngleq1(setpointx, setpointy);
     q2_diff = makeAngleq2(setpointx, setpointy);
 
@@ -232,7 +266,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; 
 }
 
 
@@ -244,26 +278,29 @@
 
 int main()
 {
-    ledr = 1;
-    ledg = 1;
-    ledb = 1;
-    
     pc.baud(115200);
     motor1_pwm.period_us(60);                // Period is 60 microseconde
     motor2_pwm.period_us(60);
     Demo.attach(&rundemo, 0.005f); 
     
-    pc.printf("\r\n\r\nDOE HET AUB!!! \r\n\r\n");
+    ledr = 1;
+    ledg = 1;
+    ledb = 1;
     
     while (true) {
-        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);
+        pc.printf("Setpointx: %0.2f, Setpointy: %02f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
         if (track == 1) {
             pc.printf("Gaat naar positie 1\r\n");
         }
         else if (track == 12) {
             pc.printf("Gaat naar positie 2\r\n");
         }
+        else if (track == 23) {
+            pc.printf("Gaat naar positie 3\r\n");
+        }
+        else if (track == 34) {
+            pc.printf("Gaat naar positie 4\r\n");
+        }
         wait(0.5f);
-   
     }
 }    
\ No newline at end of file