Diff: robot.cpp

Revision:

1:6a5065829cfc

Parent:

0:3f87ec23c3cf

Child:

2:fc5545ddf69a

--- a/robot.cpp	Thu Jan 30 10:24:20 2020 +0000
+++ b/robot.cpp	Thu Jan 30 13:58:01 2020 +0000
@@ -1,7 +1,10 @@
 #include "robot.h"
 
 robot::robot()
-    : spin(P, I, D, interval)
+    : spin(P, I, D, interval),
+      pc(USBTX, USBRX, 115200),
+      shot(kicker),
+      
 {
     omni.wheel[0].setRadian(PI/4 * 1);
     omni.wheel[1].setRadian(PI/4 * 3);
@@ -11,19 +14,24 @@
     motor[1] = new kohiMD(PA_7);
     motor[2] = new kohiMD(PA_9);
     motor[3] = new kohiMD(PB_10);
-    pid1.setInputLimits(-180,180);
-    pid1.setOutputLimits(-0.3,0.3);
-    pid1.setBias(0);
-    pid1.setMode(1);
-    pid1.setSetPoint(0);
+    spin.setInputLimits(-180, 180);
+    spin.setOutputLimits(-0.3,0.3);
+    spin.setBias(0);
+    spin.setMode(1);
+    spin.setSetPoint(0);
+    shot.setkickperiod(0.5);
+    shot.setoutputtime(0.01);
+    ui.beep(1000, 1.0);
 }
 
-void robot::chaseBall(float ball_theta, float r)
+void robot::chaseBall(float ball_theta, float ball_dis, float r)
 {
-    omni.compute();
+    theta = ball_theta + ballExtra * ((r > 0) - (r <= 0));
+    spin.setProcessValue(r);
+    omni.computeCircular( ,spin.compute());
 }
 
-void robot::searchBall()
+void robot::lostBall()
 {
     
 }
@@ -47,5 +55,20 @@
 
 void robot::shotBall()
 {
-    
+    shot.outPut;
+}
+
+void test()
+{
+    while(1) {
+        for (int i=0; i<4; i++) {
+            motor[i]->setSpeed(0.2);
+            wait(1.0);
+        }
+        for (int i=0; i<4; i++) {
+            motor[i]->setSpeed(-0.2);
+            wait(1.0);
+        }
+        
+    }
 }
\ No newline at end of file