Diff: main.cpp

Revision:

8:49f527ad271c

Parent:

7:2e578dbe3a81

Child:

9:9789153af55d

--- a/main.cpp	Tue Oct 26 15:55:52 2021 +0000
+++ b/main.cpp	Thu Oct 28 08:53:16 2021 +0000
@@ -8,8 +8,10 @@
 #include <std_msgs/Int16.h>
 
 #include "mbed.h"
-#define POINTS 900
-#define ISLAND 10
+#define POINTS 700
+#define ISLAND  702
+const double X0 = 442.5;
+const double Y0 = 628.5;
 
 
 /*--------------FUNCTION--------------*/
@@ -92,7 +94,7 @@
     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.008,0.00,0.0,0)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.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);
     /*
     RBT.addPID(0.0048,0.00,0.0000,1)->setLimit(0.5,-0.5);
@@ -112,6 +114,8 @@
     
     bool setup = false;
     bool once = false;
+    bool xy_stop = false;
+    bool rad_stop = false;
     if(!setup){
         bno.reset();
         bno.setmode(OPERATION_MODE_IMUPLUS);
@@ -123,7 +127,7 @@
     /*--------------LOOP--------------*/
     while(true){
         hoge = x;
-        pos = RBT.getStatus(false);
+        pos = RBT.getStatus(true);
         bno.get_angles();
         gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0);
         gyroA(gyro);
@@ -134,11 +138,11 @@
         else if(g_pulse < 0.0) theta += gyro-2.0*M_PI;
         else if(g_pulse == 0 and gyro < M_PI/2.0) theta += gyro;
         else if(g_pulse == 0 and gyro > 3.0*M_PI/2.0) theta += gyro-2.0*M_PI;
-        if(!once and pos.x != 0.0 and pos.y != 0.0) {
-            RBT.setPosition(0.0,0.0,0.0);
+        RBT.setPosition(pos.x,pos.y,theta);
+        if(!once){
+            RBT.setPosition(X0,Y0,0.0);
             once = true;
-            }
-        //RBT.setPosition(pos.x,pos.y,theta);
+        }
         
         //RBT.setVelocity(0.0,0.0,0.0);
         if(n<POINTS and 
@@ -150,10 +154,10 @@
         {
             n++;
             getPoint(&NextP,n);
-            NextP.theta -= M_PI/2.0;
+            //NextP.theta -= M_PI/2.0;
             getVelocity(&v,n-1);
             if(n < POINTS)getPoint(&subP,n+1);
-            if(v > 0.0) v *= 1.5;
+            if(v > 0.0) v *= 1.0;
             if(v >= 1400) v = 1400;
             //v = 20.0;
             if(n == POINTS) v = 20.0;
@@ -164,21 +168,36 @@
         RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false);
         vel.theta = RBT.PIDs[5]->getmv();
         
+        xy_stop = false;
+        rad_stop = false;
 
-        if(n == ISLAND and incircle(pos,NextP,40.0) and abs(pixel) > 40){
-            RBT.PIDs[0]->Update(pixel,0,0.02,false);
-            v = 0.0;
+        RBT.PIDs[0]->Update(pixel,0,0.02,false);
+        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;
         }
-        else if(n == ISLAND and incircle(pos,NextP,40.0)){cnt++;v = 0.0;vel.theta = 0.0;if((double)cnt > 3.0/0.02) servo.pulsewidth_us(1200);}
+        /*
+        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) 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);}
         
-        if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0;
-        if(n == POINTS and fabs(NextP.theta - pos.theta) < 0.1) vel.theta = 0.0;
+        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(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("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n);
@@ -200,8 +219,8 @@
     printf("Good!\n");
     while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta_) != EOF) {
         printf("%lf %lf %lf %lf\n",x, y, v, theta_);
-        x_info.push_back(x*2.0);
-        y_info.push_back(y*1.5);
+        x_info.push_back(x*1.0);
+        y_info.push_back(y*1.0);
         v_info.push_back(v);
         theta_info.push_back(theta_);
     }