Diff: robot.cpp

Revision:

13:d61a64c81c16

Parent:

12:19149697667d

Child:

14:8dde838ce79e

Child:

15:da88e62863a1

--- a/robot.cpp	Fri Mar 05 05:02:32 2021 +0000
+++ b/robot.cpp	Fri Mar 05 06:30:37 2021 +0000
@@ -7,10 +7,10 @@
       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);
+    omni.wheel[2].setRadian(PI/4 * 1);
+    omni.wheel[3].setRadian(PI/4 * 3);
+    omni.wheel[0].setRadian(PI/4 * 5);
+    omni.wheel[1].setRadian(PI/4 * 7);
     motor[0] = new KohiMD(PA_15);
     motor[1] = new KohiMD(PA_6);
     motor[2] = new KohiMD(PA_7);
@@ -139,5 +139,21 @@
     drib.setspeed(power);
 }
 
-//        theta = sensor.ballAngle * ballExtra * PI / 180.0;
-//            omni.computeCircular(sensor.ballRange,PI / 180.0 * sensor.ballAngle + theta, spin_power);
+void Robot::moveTest()
+{
+    while(1){
+        spin.setProcessValue(sensor_.angleLimit);
+        spin_power = spin.compute();
+
+        theta = sensor_.ballAngle * ballExtra * PI / 180.0;
+        printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(spin_power * 10));
+        omni.computeCircular(sensor_.ballRange / 100.0,PI / 180.0 * sensor_.ballAngle + theta, 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]);
+    }
+}
\ No newline at end of file