main
Dependents: 00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange
Diff: robot.cpp
- Revision:
- 1:6a5065829cfc
- Parent:
- 0:3f87ec23c3cf
- Child:
- 2:fc5545ddf69a
diff -r 3f87ec23c3cf -r 6a5065829cfc robot.cpp --- 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