Diff: robot.cpp

Revision:

4:a4f94f186ba0

Parent:

3:8e7c24edbaea

Child:

5:f5e79163d0eb

--- a/robot.cpp	Thu Jan 30 15:50:22 2020 +0000
+++ b/robot.cpp	Fri Jan 31 09:20:07 2020 +0000
@@ -1,84 +1,114 @@
 #include "robot.h"
 
-robot::robot()
-    : spin(P, I, D, interval),
+Robot::Robot() :
+      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)
       
 {
+    
     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(PA_6);
-    motor[1] = new kohiMD(PA_7);
-    motor[2] = new kohiMD(PA_9);
-    motor[3] = new kohiMD(PB_10);
+    motor[0] = new KohiMD(motor1);
+    motor[1] = new KohiMD(motor2);
+    motor[2] = new KohiMD(motor3);
+    motor[3] = new KohiMD(motor4);
     spin.setInputLimits(-180, 180);
-    spin.setOutputLimits(-0.3,0.3);
+    spin.setOutputLimits(-0.2,0.2);
     spin.setBias(0);
     spin.setMode(1);
     spin.setSetPoint(0);
-    shot.setkickperiod(0.5);
+    shot.setkickperiod(0.8);
     shot.setoutputtime(0.01);
-    ui.beep(1000, 1.0);
-}
-
-void robot::chaseBall(float ball_theta, float ball_dis, float r)
-{
-    theta = ball_theta + ballExtra * ((r > 0) - (r <= 0));
-    spin.setProcessValue(r);
-    omni.computeCircular( ,spin.compute());
-}
-
-void robot::lostBall()
-{
-    
-}
-
-void robot::moveGoal(float r, float goal_size, float goal_)
-{
-    
-}
-
-void robot::detour()
-{
+    drib.setspeed(0.0);
+    but.mode(PullUp);
     
 }
 
-void robot::outLine()
-/* ゴールから見て 
- * 右のライン -> 90°
- * 左のライン -> -90°
- * 上のライン -> 180°
- * 下のライン -> 0°
- */
+void Robot::start()
 {
-    while(/*線を踏んだか見る奴*/){
-        omni.compute
+    
+    while (true) {
+        if (!b2) 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(dip1)
+            {
+                shot.outPut();
+                if(!sensor.bluex) omni.computeCircular(1,0, spin_power);
+                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
+        {
+            for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]);
+            pc.printf("\r\n");
+        
+            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])
+        {
+            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) {
+            omni2wheel = omni.wheel[2];
+            motor[0]->setSpeed(omni.wheel[0]);
+            motor[1]->setSpeed(omni.wheel[1]);
+            motor[2]->setSpeed(-omni2wheel);
+            motor[3]->setSpeed(omni.wheel[3]);
+        } else {
+            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::shotBall()
-{
-    shot.outPut;
-}
-
-void test()
-{
-    
-    while(1) {
-        for (int i=0; i<4; i++) {
-            motor[i]->setSpeed(0.2);
-            wait(1.5);
-        }
-        wait(1.5);
-        for (int i=0; i<4; i++) {
-            motor[i]->setSpeed(-0.2);
-            wait(1.5);
-        }
-        
-    }
-}
\ No newline at end of file