main

Dependents:   00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange

Revision:
6:dee6041c3d15
Parent:
5:f5e79163d0eb
--- a/robot.cpp	Sat Feb 01 03:02:18 2020 +0000
+++ b/robot.cpp	Sat Feb 01 15:12:19 2020 +0000
@@ -34,65 +34,126 @@
     drib.setspeed(0.0);
     but.mode(PullUp);
     startb = 0;
+    linecount[0] = 0;
+    linecount[1] = 0;
+    linecount[2] = 0;
+    linecount[3] = 0;
+    lineAngle = 0;
 }
 
 void Robot::start()
 {
     
     while (true) {
-        if (!but/*2*/) startb=1;
+        pc.printf("%f\r\n", sensor.angle );
+//        sensor.blueAngle, sensor.blueRange/*, sensor.yellowAngle, sensor.yellowRange, sensor.ballAngle, sensor.ballRange */);
+        if (!/*but*/2) startb=1;
         spin.setProcessValue(sensor.angleLimit);
         theta = sensor.ballAngle * ballExtra * PI / 180.0;
         spin_power = spin.compute();
-        if (sensor.ballKeep) {
-            drib.setspeed(0.5);
+        if (sensor.ballkeepcount > 100) {
+            drib.setspeed(0.5); 
+            shot.outPut();
             if(dip1)
             {
-                shot.outPut();
+//                shot.outPut();
                 if(!sensor.bluex) omni.computeCircular(1,0, spin_power);
                 else omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, spin_power);
-        
             }
             else
             {
-                shot.outPut();
+//                shot.outPut();
                 if(!sensor.yellowx) omni.computeCircular(1,0, spin_power);
                 else omni.computeCircular(sensor.yellowRange,PI / 180.0 * sensor.yellowAngle, spin_power);
             }
         } 
         else
         {
-            for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]);
-            pc.printf("\r\n");
+//            for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]);
+//            pc.printf("\r\n");
             drib.setspeed(0.0);
             omni.computeCircular(sensor.ballRange,PI / 180.0 * sensor.ballAngle + theta, spin_power);
         }
-        pc.printf("%d ",(int)sensor.ballKeep);
+        
+//        pc.printf("%d ",(int)sensor.ballKeep);
+
+        if (sensor.ballTimeout > 1.0 && sensor.ballTimeout < 2.0)
+        {
+            omni.computeCircular(1,PI,spin_power);
+        }
+        
         if(sensor.line[0] || sensor.line[1])
         {
-            omni.computeCircular(1,-1*PI/2.0,spin_power);
-            
+            linecount[0]+=2;
         }
+        else
+        {
+               linecount[0]--;
+        }
+        if(linecount[0] && !(linecount[1] + linecount[2] + linecount[3]) ) lineAngle = -1*PI/2.0;
         if(sensor.line[2] || sensor.line[3]) 
         {
-            omni.computeCircular(1,PI/2.0,spin_power);
+            linecount[1]+=2;
         }
+        else
+        {
+            linecount[1]--;
+        }
+        if(linecount[1] && !(linecount[0] + linecount[2] + linecount[3])) lineAngle = PI/2;
         if(sensor.line[4])
         {
-             omni.computeCircular(1,0,spin_power);
+            linecount[2]+=2;
         }
+        else
+        {
+            linecount[2]--;    
+        }
+        if(linecount[2] && !(linecount[1] + linecount[0] + linecount[3])) lineAngle = 0;
         if(sensor.line[5])
         {
-             omni.computeCircular(1,PI,spin_power);
+            linecount[3]+=2;
+        }
+        else
+        {
+            linecount[3]-=2;
+        }
+        if(linecount[3] && !(linecount[1] + linecount[2] + linecount[0])) lineAngle = PI;
+        
+        if(!(linecount[0] + linecount[1] + linecount[2] + linecount[3])) lineAngle = 0;
+       
+        for(int i = 0; i < 4; i++)
+        {
+            if(linecount[i] > 50) linecount[i] = 50;
+            if(linecount[i] < 0) linecount[i] = 0;
         }
+        if(linecount[0] + linecount[1] + linecount[2] + linecount[3]) omni.computeCircular(1,lineAngle,spin_power);
+        //if(sensor.line[0] || sensor.line[1])
+//        {
+//            omni.computeCircular(1,-1*PI/2.0,spin_power);
+//            
+//        }
+//        if(sensor.line[2] || sensor.line[3]) 
+//        {
+//            omni.computeCircular(1,PI/2.0,spin_power);
+//        }
+//        if(sensor.line[4])
+//        {
+//             omni.computeCircular(1,0,spin_power);
+//        }
+//        if(sensor.line[5])
+//        {
+//             omni.computeCircular(1,PI,spin_power);
+//        }
         
         if (startb) {
+            omni0wheel = omni.wheel[0];
+            omni1wheel = omni.wheel[1];
             omni2wheel = omni.wheel[2];
             omni3wheel = omni.wheel[3];
-            motor[0]->setSpeed(omni.wheel[0]);
-            motor[1]->setSpeed(omni.wheel[1]);
-            motor[2]->setSpeed(-omni2wheel);
-            motor[3]->setSpeed(omni.wheel[3]);
+            motor[0]->setSpeed(omni0wheel);
+            motor[1]->setSpeed(omni1wheel);
+            motor[2]->setSpeed(omni2wheel);///////////////////////////////////
+            motor[3]->setSpeed(omni3wheel);
 //            motor[3]->setSpeed(-omni3wheel);
         } else {
             motor[0]->setSpeed(0);