Diff: main_demo.cpp

Revision:

2:7e86ced5841f

Parent:

1:7afdfab34bf7

Child:

3:fd1380ffb200

--- a/main_demo.cpp	Wed Oct 31 12:51:31 2018 +0000
+++ b/main_demo.cpp	Wed Oct 31 14:24:21 2018 +0000
@@ -13,6 +13,9 @@
 FastPWM motor1_pwm(D5);
 FastPWM motor2_pwm(D6);
 MODSERIAL  pc(USBTX, USBRX);
+DigitalOut ledr(LED_RED);
+DigitalOut ledg(LED_GREEN);
+DigitalOut ledb(LED_BLUE);
 
 // Tickers
 Ticker Demo;
@@ -29,8 +32,8 @@
 volatile double  error2;
 double  point1x = 200.0;
 double  point1y = 200.0;
-double  point2x = 350.0;
-double  point2y = 200.0;
+double  point2x = 200.0;
+double  point2y = 100.0;
 double  point3x = 350.0;
 double  point3y = 0.0;
 double  point4x = 200.0;
@@ -75,104 +78,61 @@
 // -------------------------------------------------------------------------
 // -------------- Determine Setpoints --------------------------------------
 // -------------------------------------------------------------------------
-double determinedemosetx(double setpointx, double setpointy)
+void determinedemoset()
 {
 
     if (setpointx < point1x){ 
-        setpointx = setpointx + 0.1;    
+        setpointx = setpointx + 0.2;    
     }
+    if(setpointy < point1y){
+        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){
-        setpointx = setpointx + 0.1;
+        setpointx = point2x;
+        setpointy = setpointy + (-0.2);          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        ledr = 0;
         track = 12;
     }
-    if (setpointx < point2x && track == 12){
-        setpointx = setpointx + 0.2;
+    if (setpointy > point2y && track == 12){
+        setpointx = point2x;
+        setpointy = setpointy + (-0.2);
     }
     
+    
     // Van punt 2 naar punt 1. 
     if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3)
     {
-        setpointx = setpointx - 0.2; 
+        //setpointx = setpointx - 0.2; 
+        //setpointy = setpointy;
+        ledr = 1;
+        ledg = 0;
         track = 23;
     }
     if (setpointy > point3y && track == 23)
     {
-        setpointx = setpointx - 0.2;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        //setpointx = setpointx - 0.2;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        //setpointy = setpointy;
         track = 23;
     } 
- 
-        /*
+/*     
     // 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. 
+        setpointy = setpointy;
         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)
-    {
-        setpointx = 80.0;
+        setpointy = setpointy;
+
     }
     */
-      return setpointx;
 }  
 
-double determinedemosety(double setpointx, double setpointy)
-{
-    // Van reference positie naar punt 1.
-    if(setpointy < point1y){
-        setpointy = setpointy + 0.2;
-    } 
-
-    // Van punt 1 naar punt 2. 
-    if (fabs(setpointx - point1x) <= 0.3 && fabs(setpointy - point1y) <= 0.3){
-        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 1. 
-    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3)
-    {
-        setpointy = setpointy;
-        track = 23;
-    }
-    if (setpointy > point3y && track == 23)
-    {
-        setpointy = setpointy;
-        track = 23;
-    }    
-    
-
- /* 
-    // Van punt 3 naar punt 4. 
-    if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
-    {
-        setpointy = setpointy;
-        track = 34;
-    }
-    if (setpointy > point3y && track == 34)
-    {
-        setpointy = setpointy;
-    }     
-    
-    
-    if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3)
-    {
-        setpointy = 141.0;
-    }
-    */ 
-    return setpointy;
-    
-}
 
 // -----------------------------------------------------------------    
 // --------------------------- PI controllers ----------------------
@@ -240,8 +200,7 @@
 // -----------------------------------------------
 void motoraansturing()
 {
-    setpointx = determinedemosetx(setpointx, setpointy);
-    setpointy = determinedemosety(setpointx, setpointy);
+    determinedemoset();
     q1_diff = makeAngleq1(setpointx, setpointy);
     q2_diff = makeAngleq2(setpointx, setpointy);
 
@@ -268,6 +227,10 @@
 
 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);