Diff: robot.cpp

Revision:

16:e3846421554b

Parent:

15:da88e62863a1

diff -r da88e62863a1 -r e3846421554b robot.cpp
--- a/robot.cpp	Sat Mar 06 07:26:44 2021 +0000
+++ b/robot.cpp	Sun Mar 07 12:23:16 2021 +0000
@@ -215,26 +215,42 @@
              omni.computeCircular(1,PI,spin_power);
              lineflag = 1;
         }
-        if(lineflag == 0 && !sensor_.ballkeepcount){
+        
+        if(lineflag == 0 && (!sensor_.ballkeepcount || sensor_.ballRange)){
             theta = ((sensor_.ballAngle>0) - (sensor_.ballAngle<0)) * (2.0/(2-fabs(sensor_.ballAngle)/180) - 1) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
-            omni.computeCircular(/*0*/1.1*sensor_.ballRange / 100.0,/*0*/PI / 180.0 * sensor_.ballAngle + theta, spin_power);
+            omni.computeCircular(/*0*/ 0.5,/*0*/PI / 180.0 * sensor_.ballAngle + theta, spin_power);
         }
-        else if(sensor_.ballkeepcount)
+        
+        if(lineflag == 0 && sensor_.ballkeepcount)
         {
-        spin_power = sgoal.compute();
+//            spin_power = sgoal.compute();
             if(sensor_.team)
             {
-                omni.computeCircular(sensor_.blueRange / 100.0/*0.6*/,sensor_.blueAngle,spin_power);
+//                spin_power = sensor_.blueAngle / 180.0 * 0.4;
+                omni.computeCircular(/*sensor_.blueRange / 100.0*//*0.45*/0.45,PI / 180.0 * sensor_.blueAngle,spin_power);
             }
             else
             {
-                omni.computeCircular(sensor_.yellowRange / 100.0/*0.6*/,sensor_.yellowAngle,spin_power);
+//                spin_power = sensor_.yellowAngle / 180.0 * 0.4;
+                omni.computeCircular(/*sensor_.yellowRange / 100.0*/0.45,PI / 180.0 * sensor_.yellowAngle,spin_power);
             }
-            if(lineflag) kickCheck();
         }
+        
+        if(lineflag == 0 && !sensor_.ballkeepcount && !sensor_.ballRange)
+        {
+            if(sensor_.team)
+            {
+                omni.computeCircular(0.4,PI / 180.0 * sensor_.yellowAngle,spin_power);
+            }
+            else
+            {
+                omni.computeCircular(0.4,PI / 180.0 * sensor_.blueAngle,spin_power);
+            }
+        }
+        if((lineflag || (sensor_.yellowRange < 60)) && sensor_.ballkeepcount) kickCheck();
         lineflag = 0;
 //        printf("r:%d %d l:%d %d b:%d f:%d\r\n",sensor_.line[0],sensor_.line[1],sensor_.line[2],sensor_.line[3],sensor_.line[4],sensor_.line[5]);
-//        printf("%d %d %d %d\r\n",(int)(sensor_.b0),(int)sensor_.tgl1,(int)sensor_.tgl2,(int)sensor_.tgl3);
+//        printf("%d %d %d %d\r\n",(int)(sensor_.b0),(int)sensor_.test,(int)sensor_.team,(int)sensor_.start);
         for (int i=0; i<4; i++) {
             thisSpeed[i] = omni.wheel[i];
         }