Diff: main.cpp

Revision:

1:aa9cc4250220

Parent:

0:f80bb69e638e

Child:

2:4e7ec0816a91

--- a/main.cpp	Wed Oct 13 08:47:48 2021 +0000
+++ b/main.cpp	Thu Oct 21 16:16:24 2021 +0000
@@ -10,7 +10,7 @@
 
 
 /*--------------FUNCTION--------------*/
-void init();
+bool init();
 void route_read();
 void getPoint(Position *p,int n);
 void getVelocity(double *v,int n);
@@ -41,10 +41,12 @@
 Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02);
 
 int main(){
+    
+PwmOut ok(PC_6);
 
 /*--------------SETUP--------------*/
-scrp.addCMD(6,checkMOT12);
-scrp.addCMD(7,checkMOT34);
+scrp.addCMD(7,checkMOT12);
+scrp.addCMD(8,checkMOT34);
 AKASHIKOSEN.setCWID(1,2,3,4);
 AKASHIKOSEN.setSWID(5,6,7,8);
     
@@ -61,14 +63,23 @@
 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.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);
+RBT.addPID(0.0048,0.00,0.0000,2)->setLimit(0.5,-0.5);
+RBT.addPID(0.0048,0.00,0.00,3)->setLimit(0.5,-0.5);
+RBT.addPID(0.0048,0.00,0.00,4)->setLimit(0.5,-0.5);
 RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
+*/
+MOT12 = false,MOT34 = false;
 
-init();
-
+while(init());
+printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34);
 route_read();
 
 wait(1);
-
+ok = 1.0;
 
 bool setup = false;
 if(!setup){
@@ -78,9 +89,10 @@
 }
     
 RBT.START();
+RBT.setPosition(0.0,0.0,0.0);
 /*--------------LOOP--------------*/
 while(true){
-    pos = RBT.getStatus();
+    pos = RBT.getStatus(true);
     
     bno.get_angles();
     gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0);
@@ -97,7 +109,7 @@
     
     //RBT.setVelocity(0.0,0.0,0.0);
     if(n<POINTS and 
-        ( (incircle(pos,NextP,30.0) and n < POINTS and n != 1000) or 
+        ( (incircle(pos,NextP,40.0) and n < POINTS and n != 1000) 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)
         ) 
@@ -105,19 +117,21 @@
     {
         n++;
         getPoint(&NextP,n);
+        NextP.theta -= M_PI/2.0;
         getVelocity(&v,n-1);
         if(n < POINTS)getPoint(&subP,n+1);
-        if(v > 0.0) v *= 1.7;
+        if(v > 0.0) v *= 1.5;
         if(v >= 1400) v = 1400;
-        //v = 200.0;
-        if(n == POINTS) v = 50.0;
+        //v = 20.0;
+        if(n == POINTS) v = 20.0;
         printf("%d:pass\n",n);
     }
     if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0;
     double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x);
-    RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02);
-    RBT.sendVelocity(v*cos(etheta),v*sin(etheta),RBT.PIDs[5]->getGain());
-    //RBT.sendVelocity(200.0,200.0,0.0);
+    RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false);
+    RBT.sendVelocity(v*cos(etheta),v*sin(etheta),RBT.PIDs[5]->getmv());
+    //printf("%lf,%lf\n",pos.theta,NextP.theta);
+    //RBT.sendVelocity(200.0,-200.0,0.0);
     printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n);
     RBT.LOOP();
 }
@@ -136,8 +150,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);
-        y_info.push_back(y);
+        x_info.push_back(x*2.0);
+        y_info.push_back(y*1.5);
         v_info.push_back(v);
         theta_info.push_back(theta_);
     }
@@ -199,8 +213,10 @@
     return true;
 }
 
-void init(){
-    MOT12 = false,MOT34 = false;
+bool init(){
+    wait(0.25);
     scrp.send1(255,5,1);
-    while(!MOT12 or !MOT34);
+    wait(0.25);
+    scrp.send1(255,6,1);
+    return (!MOT12 or !MOT34);
 }
\ No newline at end of file