Diff: robot.cpp

Revision:

5:f5e79163d0eb

Parent:

4:a4f94f186ba0

Child:

6:dee6041c3d15

Child:

7:dcc0faa86da7

--- a/robot.cpp	Fri Jan 31 09:20:07 2020 +0000
+++ b/robot.cpp	Sat Feb 01 03:02:18 2020 +0000
@@ -25,7 +25,7 @@
     motor[2] = new KohiMD(motor3);
     motor[3] = new KohiMD(motor4);
     spin.setInputLimits(-180, 180);
-    spin.setOutputLimits(-0.2,0.2);
+    spin.setOutputLimits(-0.4,0.4);
     spin.setBias(0);
     spin.setMode(1);
     spin.setSetPoint(0);
@@ -33,14 +33,14 @@
     shot.setoutputtime(0.01);
     drib.setspeed(0.0);
     but.mode(PullUp);
-    
+    startb = 0;
 }
 
 void Robot::start()
 {
     
     while (true) {
-        if (!b2) startb=1;
+        if (!but/*2*/) startb=1;
         spin.setProcessValue(sensor.angleLimit);
         theta = sensor.ballAngle * ballExtra * PI / 180.0;
         spin_power = spin.compute();
@@ -50,21 +50,21 @@
             {
                 shot.outPut();
                 if(!sensor.bluex) omni.computeCircular(1,0, spin_power);
-                omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, -spin_power);
+                else omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, spin_power);
         
             }
             else
             {
                 shot.outPut();
                 if(!sensor.yellowx) omni.computeCircular(1,0, spin_power);
-                omni.computeCircular(sensor.yellowRange,PI / 180.0 * sensor.yellowAngle, -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");
-        
+            drib.setspeed(0.0);
             omni.computeCircular(sensor.ballRange,PI / 180.0 * sensor.ballAngle + theta, spin_power);
         }
         pc.printf("%d ",(int)sensor.ballKeep);
@@ -88,10 +88,12 @@
         
         if (startb) {
             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[3]->setSpeed(-omni3wheel);
         } else {
             motor[0]->setSpeed(0);
             motor[1]->setSpeed(0);