Diff: robot.cpp

Revision:

14:8dde838ce79e

Parent:

13:d61a64c81c16

--- a/robot.cpp	Fri Mar 05 06:30:37 2021 +0000
+++ b/robot.cpp	Fri Mar 05 14:01:03 2021 +0000
@@ -102,6 +102,22 @@
         spin.setProcessValue(sensor_.angleLimit);
         spin_power = spin.compute();
         omni.computeCircular(0,0,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);
+        }
         for (int i=0; i<4; i++) {
             thisSpeed[i] = omni.wheel[i];
         }
@@ -142,11 +158,14 @@
 void Robot::moveTest()
 {
     while(1){
+        printf("%d", (int)sensor_.irVal * 100);
         spin.setProcessValue(sensor_.angleLimit);
         spin_power = spin.compute();
 
-        theta = sensor_.ballAngle * ballExtra * PI / 180.0;
-        printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(spin_power * 10));
+//        theta = sensor_.ballAngle * ballExtra * PI / 180.0 * ((120-sensor_.ballRange)/240.0) * (fabs(sensor_.ballAngle) > 25.0) ;
+//        theta =30 * sensor_.ballAngle*sensor_.ballAngle * 0.5 * PI / 180.0 / (1.5*sensor_.ballRange + 1);
+        theta = (sensor_.ballAngle / 180.0) * (sensor_.ballAngle / 180.0) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
+        printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(fabs(sensor_.ballAngle) > 25.0));
         omni.computeCircular(sensor_.ballRange / 100.0,PI / 180.0 * sensor_.ballAngle + theta, spin_power);
         for (int i=0; i<4; i++) {
             thisSpeed[i] = omni.wheel[i];