Diff: main.cpp

Revision:

5:aee4416b0d4f

Parent:

4:0038da6cdc9a

Child:

6:e7b9b1d46852

--- a/main.cpp	Sun Oct 24 04:48:27 2021 +0000
+++ b/main.cpp	Mon Oct 25 08:01:27 2021 +0000
@@ -45,14 +45,20 @@
 Ticker rosspin;
 
 SDFileSystem sd(PB_15, PB_14, PB_13, PB_1,"sd");// mosi, miso, sclk, cs
-ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);
+//ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);
+ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,0x0807f800);
 Robot AKASHIKOSEN(50.8,25.4,322.5,259.75);
 Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02);
 
-ros::Subscriber<std_msgs::Int16> sub("pixel", &getPixel);
+ros::Subscriber<std_msgs::Int16> sub("unti", &getPixel);
+
+int x=1;
 
 int main(){
-    
+
+    DigitalOut hoge(PB_2);
+    hoge = x;  
+      
     PwmOut ok(PC_6);
 
 /*--------------SETUP--------------*/
@@ -60,7 +66,7 @@
     nh.getHardware()->setBaud(115200);
     nh.initNode();
     nh.subscribe(sub);
-    rosspin.attach_us(&spinonce,100);
+    rosspin.attach_us(&spinonce,1000);
         
     scrp.addCMD(7,checkMOT12);
     scrp.addCMD(8,checkMOT34);
@@ -100,6 +106,7 @@
     ok = 1.0;
     
     bool setup = false;
+    bool once = false;
     if(!setup){
         bno.reset();
         bno.setmode(OPERATION_MODE_IMUPLUS);
@@ -110,6 +117,7 @@
     RBT.setPosition(0.0,0.0,0.0);
     /*--------------LOOP--------------*/
     while(true){
+        hoge = x;
         pos = RBT.getStatus(true);
         bno.get_angles();
         gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0);
@@ -121,14 +129,17 @@
         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) {
+            RBT.setPosition(0.0,0.0,0.0);
+            once = true;
+            }
         RBT.setPosition(pos.x,pos.y,theta);
         
         //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)
+                ( 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)
             ) 
         )
         {
@@ -148,11 +159,12 @@
         RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false);
         vel.theta = RBT.PIDs[5]->getmv();
         
-        if(n == ISLAND and incircle(pos,NextP,10.0) and abs(pixel) > 10){
+        if(n == ISLAND and incircle(pos,NextP,40.0) and abs(pixel) > 10){
             RBT.PIDs[0]->Update(pixel,0,0.02,false);
+            v = 0.0;
             vel.theta = RBT.PIDs[0]->getmv();
         }
-        else {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;
@@ -162,6 +174,7 @@
         
         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);
+        nh.spinOnce();
         RBT.LOOP();
     }
 }
@@ -247,6 +260,7 @@
     scrp.send1(255,5,1);
     wait(0.25);
     scrp.send1(255,6,1);
+    printf("aaaa");
     return (!MOT12 or !MOT34);
 }
 void spinonce()
@@ -256,4 +270,9 @@
 
 void getPixel(const std_msgs::Int16& pix){
     pixel = pix.data;
+    if (pixel > 0){
+        x = 0;    
+    } else {
+        x = 1;
     }
+}