Diff: robot.cpp

Revision:

12:19149697667d

Parent:

11:39f44a1dc256

Child:

13:d61a64c81c16

--- a/robot.cpp	Fri Mar 05 00:00:17 2021 +0000
+++ b/robot.cpp	Fri Mar 05 05:02:32 2021 +0000
@@ -34,21 +34,20 @@
         spin.setProcessValue(sensor_.angleLimit);
         theta = sensor_.ballAngle * ballExtra * PI / 180.0;
         spin_power = spin.compute();
-        if (sensor_.ballKeep) {
+        if (/*sensor_.ballKeep*/0) {
             /*ボールをつかんでるなら*/
 //            drib.setspeed(0.5);
             if(Team)
             {
                 /*あおに向かうよ*/
-                /*if(!sensor.bluex)*/ omni.computeCircular(1,0,0/* spin_power*/);
-//                else omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, spin_power);
+//                /*if(!sensor.bluex)*/ omni.computeCircular(1,0,0/* spin_power*/);
+//                omni.computeCircular(sensor_.blueRange,PI / 180.0 * sensor_.blueAngle, spin_power);
         
             }
             else
             {
                 /*きいろに向かうよ*/
-                /*if(!sensor.yellowx)*/ omni.computeCircular(1,0,0/* spin_power*/);
-                /*else*/ omni.computeCircular(sensor_.yellowRange,PI / 180.0 * sensor_.yellowAngle, spin_power);
+//                /*if(!sensor.yellowx)*/ omni.computeCircular(1,0,0/* spin_power*/);
             }
         }
         else
@@ -57,7 +56,8 @@
 //            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);
+//            omni.computeCircular(sensor_.ballRange, (PI / 180.0) * sensor_.ballAngle + theta, spin_power);
+            omni.computeCircular(sensor_.ballRange, sensor_.ballAngle, spin_power);
         }
 //        printf("%d ",(int)sensor.ballKeep);
         if(sensor_.line[0] || sensor_.line[1])
@@ -88,14 +88,30 @@
             shot.outPut();
 
         } else {
-//            motor[0]->setSpeed(0);
-//            motor[1]->setSpeed(0);
-//            motor[2]->setSpeed(0);
-//            motor[3]->setSpeed(0);
+            motor[0]->setSpeed(0);
+            motor[1]->setSpeed(0);
+            motor[2]->setSpeed(0);
+            motor[3]->setSpeed(0);
         }
     }
 }
 
+void Robot::pidtest()
+{
+    while(1){
+        spin.setProcessValue(sensor_.angleLimit);
+        spin_power = spin.compute();
+        omni.computeCircular(0,0,spin_power);
+        for (int i=0; i<4; i++) {
+            thisSpeed[i] = omni.wheel[i];
+        }
+        motor[0]->setSpeed(thisSpeed[0]);
+        motor[1]->setSpeed(thisSpeed[1]);
+        motor[2]->setSpeed(-1*thisSpeed[2]);
+        motor[3]->setSpeed(-1*thisSpeed[3]);
+    }
+}
+
 void Robot::motorCheck(int motorNumber, float power)
 {
     _motorNumber = motorNumber;