Diff: robot.cpp

Revision:

9:904dac75a729

Parent:

8:713bbc1fb58f

Child:

11:39f44a1dc256

--- a/robot.cpp	Thu Mar 04 01:20:51 2021 +0000
+++ b/robot.cpp	Thu Mar 04 11:58:52 2021 +0000
@@ -4,84 +4,75 @@
       spin(P, I, D, interval),
       pc(USBTX, USBRX, 115200),
       shot(kicker),
-      drib(ESC),
-      
-      but(USER_BUTTON),
-      dip1(dip1in),
-      dip2(dip2in),
-      dip3(dip3in),
-      b1(b1in),
-      b2(b2in),
-      lcd(PB_9, PB_8)
-      
+      drib(ESCpin)
 {
     
     omni.wheel[0].setRadian(PI/4 * 1);
     omni.wheel[1].setRadian(PI/4 * 3);
     omni.wheel[2].setRadian(PI/4 * 5);
     omni.wheel[3].setRadian(PI/4 * 7);
-    motor[0] = new KohiMD(motor1);
-    motor[1] = new KohiMD(motor2);
-    motor[2] = new KohiMD(motor3);
-    motor[3] = new KohiMD(motor4);
+    motor[0] = new KohiMD(PA_15);
+    motor[1] = new KohiMD(PA_6);
+    motor[2] = new KohiMD(PA_7);
+    motor[3] = new KohiMD(PB_6);
     spin.setInputLimits(-180, 180);
     spin.setOutputLimits(-0.4,0.4);
     spin.setBias(0);
     spin.setMode(1);
     spin.setSetPoint(0);
-    shot.setkickperiod(0.8);
-    shot.setoutputtime(0.01);
+    shot.setkickperiod(2.0);
+    shot.setoutputtime(0.1);
     drib.setspeed(0.0);
-    but.mode(PullUp);
     startb = 0;
 }
 
-void Robot::start()
+void Robot::start(uint8_t Team, uint8_t Algorithm)
 {
     
-    while (true) {
-        if (!but/*2*/) startb=1;
-        spin.setProcessValue(sensor.angleLimit);
-        theta = sensor.ballAngle * ballExtra * PI / 180.0;
+    while (1) {
+        startb=1;
+        spin.setProcessValue(sensor_.angleLimit);
+        theta = sensor_.ballAngle * ballExtra * PI / 180.0;
         spin_power = spin.compute();
-        if (sensor.ballKeep) {
+        if (sensor_.ballKeep) {
+            /*ボールをつかんでるなら*/
 //            drib.setspeed(0.5);
-            if(dip1)
+            if(Team)
             {
-//                shot.outPut();
+                /*あおに向かうよ*/
                 /*if(!sensor.bluex)*/ omni.computeCircular(1,0,0/* spin_power*/);
 //                else omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, spin_power);
         
             }
             else
             {
-//                shot.outPut();
+                /*きいろに向かうよ*/
                 /*if(!sensor.yellowx)*/ omni.computeCircular(1,0,0/* spin_power*/);
-//                else 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");
+//            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);
         }
-        pc.printf("%d ",(int)sensor.ballKeep);
-        if(sensor.line[0] || sensor.line[1])
+//        printf("%d ",(int)sensor.ballKeep);
+        if(sensor_.line[0] || sensor_.line[1])
         {
             omni.computeCircular(1,-1*PI/2.0,spin_power);
-            
         }
-        if(sensor.line[2] || sensor.line[3]) 
+        if(sensor_.line[2] || sensor_.line[3]) 
         {
             omni.computeCircular(1,PI/2.0,spin_power);
         }
-        if(sensor.line[4])
+        if(sensor_.line[4])
         {
              omni.computeCircular(1,0,spin_power);
         }
-        if(sensor.line[5])
+        if(sensor_.line[5])
         {
              omni.computeCircular(1,PI,spin_power);
         }
@@ -89,47 +80,37 @@
         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[0]->setSpeed(omni.wheel[0]);
+//            motor[1]->setSpeed(omni.wheel[1]);
+//            motor[2]->setSpeed(omni.wheel[2]);
+//            motor[3]->setSpeed(omni.wheel[3]);
 //            motor[3]->setSpeed(-omni3wheel);
             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);
         }
-        
-        
-        if (!startb) {
-            if (!b1) {
-                if (dip3) {
-                    sensor.jy.jyroReset();
-                } else {
-                    shot.outPut();
-                }
-            }
-        }
-            
     }
 }
 
 void Robot::motorCheck(int motorNumber, float power)
 {
     _motorNumber = motorNumber;
-    for(i = 3; i >= 0; i++)
+    for(i = 0; i < 4; i++)
     {
-        if(_motorNumber % 2) motor[i] -> setSpeed(power);
-        _motorNumber /= 2;
+        if(i == _motorNumber) motor[i]->setSpeed(power);
+        else motor[i]->setSpeed(0);
     }
 }
 
-void Robot::motorStop()
+void Robot::motorStop(double pwm)
 {
-    for(i=0; i<4; i++) motor[i] -> setSpeed(0);
+    motorSpeed = 0.5;
+    printf("%d\r\n", (int)(motorSpeed*10));
+    for(i=0; i<4; i++) motor[i]->setSpeed(motorSpeed);
 }
 
 void Robot::kickCheck()