Diff: main.cpp

Revision:

10:5c2adb59a0f0

Parent:

9:9789153af55d

Child:

11:05c132be4f82

--- a/main.cpp	Fri Oct 29 16:31:25 2021 +0000
+++ b/main.cpp	Sat Oct 30 04:50:17 2021 +0000
@@ -8,9 +8,9 @@
 #include <std_msgs/Int16.h>
 
 #include "mbed.h"
-#define POINTS 1900
-#define ISLAND 1898
-#define PARK 700
+#define POINTS 220
+#define ISLAND 190
+#define PARK 70
 const double X0 = 442.5;
 const double Y0 = 628.5;
 
@@ -42,7 +42,7 @@
 bool gyro_1 = false,gyro_2 = false;
 long long g_pulse;
 bool MOT12 = false,MOT34 = false;
-
+bool ros_ = false;
 BNO055 bno(PB_3, PB_10);
 
 ros::NodeHandle nh;
@@ -69,7 +69,6 @@
     DigitalIn Lim_L(PA_4,PullDown);
 
 /*--------------SETUP--------------*/
-
     servo.period_ms(2048);
     servo.pulsewidth_us(1500);
     
@@ -110,6 +109,7 @@
     
     while(init());
     printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34);
+    
     route_read();
     
     wait(1);
@@ -128,6 +128,7 @@
     RBT.START();
     RBT.setPosition(0.0,0.0,0.0);
     /*--------------LOOP--------------*/
+    while(Lim_R == 1 and Lim_L == 1){};
     while(true){
         hoge = x;
         pos = RBT.getStatus(true);
@@ -149,21 +150,22 @@
         
         //RBT.setVelocity(0.0,0.0,0.0);
         if(n<POINTS and 
-            ( (incircle(pos,NextP,60.0) and n < POINTS  and n != ISLAND and n != PARK) or 
-                ( incircle(pos,NextP,10.0) and n == 2000 ) or
+            ( (incircle(pos,NextP,40.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)
             ) 
         )
         {
             n++;
             getPoint(&NextP,n);
-            if(n < 100) NextP.theta = 0.0;
+            if(n < 10) NextP.theta = 0.0;
             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 > 700) v = 300;
             //v = 20.0;
-            if(n == POINTS) v = 20.0;
+            if(n == POINTS) v = 100.0;
             printf("%d:pass\n",n);
         }
         
@@ -175,7 +177,7 @@
         rad_stop = false;
 
         RBT.PIDs[0]->Update(pixel,0.1,0.02,false);
-        servo.pulsewidth_us(1500);
+        if(n < ISLAND or n == POINTS) 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();
@@ -185,18 +187,21 @@
             cnt++;
             xy_stop = true;
             rad_stop = true;
-            if((double)cnt > 2.5/0.02)n++;
-            if((double)cnt > 2.0/0.02) servo.pulsewidth_us(1170);
+            if((double)cnt > 2.0/0.02) {v=150.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(n == POINTS and incircle(pos,NextP,1.0)) xy_stop = true;
+        if(n == POINTS and incircle(pos,NextP,10.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++;}}
+        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++;}}
         
         if(xy_stop) {vel.x = 0.0;vel.y = 0.0;}
         if(rad_stop) {vel.theta = 0.0;}
@@ -300,6 +305,7 @@
 
 void getPixel(const std_msgs::Int16& pix){
     pixel = pix.data;
+    ros_ = true;
     if (pixel > 0){
         x = 0;    
     } else {