Diff: main.cpp

Revision:

9:9789153af55d

Parent:

8:49f527ad271c

Child:

10:5c2adb59a0f0

--- a/main.cpp	Thu Oct 28 08:53:16 2021 +0000
+++ b/main.cpp	Fri Oct 29 16:31:25 2021 +0000
@@ -8,8 +8,9 @@
 #include <std_msgs/Int16.h>
 
 #include "mbed.h"
-#define POINTS 700
-#define ISLAND  702
+#define POINTS 1900
+#define ISLAND 1898
+#define PARK 700
 const double X0 = 442.5;
 const double Y0 = 628.5;
 
@@ -64,6 +65,8 @@
       
     PwmOut servo(PA_0);
     PwmOut ok(PC_6);
+    DigitalIn Lim_R(PB_0,PullDown);
+    DigitalIn Lim_L(PA_4,PullDown);
 
 /*--------------SETUP--------------*/
 
@@ -146,19 +149,19 @@
         
         //RBT.setVelocity(0.0,0.0,0.0);
         if(n<POINTS and 
-            ( (incircle(pos,NextP,40.0) and n < POINTS and n != 1000 and n != ISLAND) or 
-                ( incircle(pos,NextP,10.0) and n == 1000 ) 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)
+            ( (incircle(pos,NextP,60.0) and n < POINTS  and n != ISLAND and n != PARK) or 
+                ( incircle(pos,NextP,10.0) and n == 2000 ) 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)
             ) 
         )
         {
             n++;
             getPoint(&NextP,n);
-            //NextP.theta -= M_PI/2.0;
+            if(n < 100) NextP.theta = 0.0;
             getVelocity(&v,n-1);
             if(n < POINTS)getPoint(&subP,n+1);
             if(v > 0.0) v *= 1.0;
-            if(v >= 1400) v = 1400;
+            //if(n > 700) v = 300;
             //v = 20.0;
             if(n == POINTS) v = 20.0;
             printf("%d:pass\n",n);
@@ -171,35 +174,35 @@
         xy_stop = false;
         rad_stop = false;
 
-        RBT.PIDs[0]->Update(pixel,0,0.02,false);
+        RBT.PIDs[0]->Update(pixel,0.1,0.02,false);
+        servo.pulsewidth_us(1500);
         if(n == ISLAND and incircle(pos,NextP,30.0) and abs(pixel) > 40){
             xy_stop = true;
             vel.theta = RBT.PIDs[0]->getmv();
-            servo.pulsewidth_us(1500);
             cnt = 0;
         }
-        /*
-        if(n == ISLAND and incircle(pos,NextP,30.0) and pixel < -40){
+        else if(n == ISLAND and incircle(pos,NextP,30.0)){
+            cnt++;
             xy_stop = true;
-            vel.theta = 0.5;
-            cnt = 0;
-        }
-        else if(n == ISLAND and incircle(pos,NextP,30.0) and pixel > 40){
-            xy_stop = true;
-            vel.theta = -0.5;
-            cnt = 0;
-        }*/
-        else if(n == ISLAND and incircle(pos,NextP,30.0)){cnt++;xy_stop = true;rad_stop = true;if((double)cnt > 3.0/0.02) servo.pulsewidth_us(1170);}
+            rad_stop = true;
+            if((double)cnt > 2.5/0.02)n++;
+            if((double)cnt > 2.0/0.02) servo.pulsewidth_us(1170);
+            }
         
         if(n == POINTS and incircle(pos,NextP,1.0)) xy_stop = true;
         if(n == POINTS and fabs(NextP.theta - pos.theta) < 0.1) rad_stop = true;;
         
         vel.x = v*cos(etheta);
         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 > 10.0/0.02) {v=150.0;cnt = 0;n++;}}
+        
         if(xy_stop) {vel.x = 0.0;vel.y = 0.0;}
         if(rad_stop) {vel.theta = 0.0;}
         
         RBT.sendVelocity(vel.x,vel.y,vel.theta);
+        //printf("L:%d R:%d\n",Lim_L,Lim_R);
         printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n);
         nh.spinOnce();
         RBT.LOOP();