Diff: main.cpp

Revision:

6:e7b9b1d46852

Parent:

5:aee4416b0d4f

Child:

7:2e578dbe3a81

--- a/main.cpp	Mon Oct 25 08:01:27 2021 +0000
+++ b/main.cpp	Mon Oct 25 08:29:01 2021 +0000
@@ -87,7 +87,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.01,0.00,0.0,0)->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.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);
@@ -118,7 +118,7 @@
     /*--------------LOOP--------------*/
     while(true){
         hoge = x;
-        pos = RBT.getStatus(true);
+        pos = RBT.getStatus(false);
         bno.get_angles();
         gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0);
         gyroA(gyro);
@@ -129,11 +129,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) {
+        if(!once and pos.x != 0.0 and pos.y != 0.0) {
             RBT.setPosition(0.0,0.0,0.0);
             once = true;
             }
-        RBT.setPosition(pos.x,pos.y,theta);
+        //RBT.setPosition(pos.x,pos.y,theta);
         
         //RBT.setVelocity(0.0,0.0,0.0);
         if(n<POINTS and 
@@ -164,7 +164,7 @@
             v = 0.0;
             vel.theta = RBT.PIDs[0]->getmv();
         }
-        else if(n == ISLAND and incircle(pos,NextP,40.0)){v = 0.0;vel.theta = 0.0;/*RBT.sendPWM(1,9)*/;}
+        else if(n == ISLAND and incircle(pos,NextP,40.0)){v = 0.0;vel.theta = 0.0;/*RBT.sendPWM(1,9);*/}
         
         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;