Diff: main.cpp

Revision:

11:05c132be4f82

Parent:

10:5c2adb59a0f0

Child:

12:ed227eb82214

--- a/main.cpp	Sat Oct 30 04:50:17 2021 +0000
+++ b/main.cpp	Sat Oct 30 11:14:05 2021 +0000
@@ -95,7 +95,7 @@
     RBT.addPID(0.0048,0.001,0.00008,2)->setLimit(0.5,-0.5);
     RBT.addPID(0.0048,0.001,0.00008,3)->setLimit(0.5,-0.5);
     RBT.addPID(0.0048,0.001,0.00008,4)->setLimit(0.5,-0.5);
-    RBT.addPID(0.8,0.00,0.0,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
+    RBT.addPID(0.8,0.00,0.0,5)->setLimit(2.0*M_PI/4.0,-2.0*M_PI/4.0);
     RBT.addPID(0.007,0.00,0.0,0)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
     //RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
     /*
@@ -150,7 +150,7 @@
         
         //RBT.setVelocity(0.0,0.0,0.0);
         if(n<POINTS and 
-            ( (incircle(pos,NextP,40.0) and n < POINTS  and n != ISLAND and n != PARK) or 
+            ( (incircle(pos,NextP,50.0) and n < POINTS  and n != ISLAND and n != PARK) or 
                 ( incircle(pos,NextP,10.0) and n == PARK-1 ) or
                 ( sqrt( (NextP.x-pos.x)*(NextP.x-pos.x)+(NextP.y-pos.y)*(NextP.y-pos.y) ) > sqrt( (subP.x-pos.x)*(subP.x-pos.x)+(subP.y-pos.y)*(subP.y-pos.y) ) and n < POINTS and n != ISLAND and n != PARK)
             ) 
@@ -162,7 +162,11 @@
             getVelocity(&v,n-1);
             if(n < POINTS)getPoint(&subP,n+1);
             if(v > 0.0) v *= 1.0;
-            if(n < ISLAND) v*= 1.5;
+            if(n < PARK) v*= 1.9;
+            if(n < 120 and n > PARK) v*= 2.0;
+            if(160 > n and n >= 120) v*= 1.7;
+            if(180 > n and n >= 160) v*= 2.5;
+            if(n >= 180) v*= 3.0;
             //if(n > 700) v = 300;
             //v = 20.0;
             if(n == POINTS) v = 100.0;
@@ -187,11 +191,11 @@
             cnt++;
             xy_stop = true;
             rad_stop = true;
-            if((double)cnt > 2.0/0.02) {v=150.0;servo.pulsewidth_us(1170);n++;}
+            if((double)cnt > 2.0/0.02) {v=300.0;servo.pulsewidth_us(1170);n++;}
             }
         if(n > ISLAND and n <= ISLAND + 5) {
             cnt++;
-            if((double)cnt < 4.0/0.02) {v=150.0;servo.pulsewidth_us(1170);xy_stop = true;rad_stop = true;}
+            if((double)cnt < 4.0/0.02) {v=300.0;servo.pulsewidth_us(1170);xy_stop = true;rad_stop = true;}
         }
         
         if(n == POINTS and incircle(pos,NextP,10.0)) xy_stop = true;
@@ -201,7 +205,7 @@
         vel.y = v*sin(etheta);
         
         if(n == PARK and Lim_R == 1 and Lim_L == 1) {vel.x = 100.0; vel.y = 0.0; ok = 1;cnt = 0;}
-        else if(n == PARK) {xy_stop = true;rad_stop = true;ok = 0;cnt++;if((double)cnt > 12.0/0.02) {v=150.0;cnt = 0;n++;}}
+        else if(n == PARK) {xy_stop = true;rad_stop = true;ok = 0;cnt++;if((double)cnt > 10.0/0.02) {v=300.0;cnt = 0;n++;}}
         
         if(xy_stop) {vel.x = 0.0;vel.y = 0.0;}
         if(rad_stop) {vel.theta = 0.0;}