main
Dependents: 00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange
robot.cpp@1:6a5065829cfc, 2020-01-30 (annotated)
- Committer:
- piroro4560
- Date:
- Thu Jan 30 13:58:01 2020 +0000
- Revision:
- 1:6a5065829cfc
- Parent:
- 0:3f87ec23c3cf
- Child:
- 2:fc5545ddf69a
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 0:3f87ec23c3cf | 1 | #include "robot.h" |
piroro4560 | 0:3f87ec23c3cf | 2 | |
piroro4560 | 0:3f87ec23c3cf | 3 | robot::robot() |
piroro4560 | 1:6a5065829cfc | 4 | : spin(P, I, D, interval), |
piroro4560 | 1:6a5065829cfc | 5 | pc(USBTX, USBRX, 115200), |
piroro4560 | 1:6a5065829cfc | 6 | shot(kicker), |
piroro4560 | 1:6a5065829cfc | 7 | |
piroro4560 | 0:3f87ec23c3cf | 8 | { |
piroro4560 | 0:3f87ec23c3cf | 9 | omni.wheel[0].setRadian(PI/4 * 1); |
piroro4560 | 0:3f87ec23c3cf | 10 | omni.wheel[1].setRadian(PI/4 * 3); |
piroro4560 | 0:3f87ec23c3cf | 11 | omni.wheel[2].setRadian(PI/4 * 5); |
piroro4560 | 0:3f87ec23c3cf | 12 | omni.wheel[3].setRadian(PI/4 * 7); |
piroro4560 | 0:3f87ec23c3cf | 13 | motor[0] = new kohiMD(PA_6); |
piroro4560 | 0:3f87ec23c3cf | 14 | motor[1] = new kohiMD(PA_7); |
piroro4560 | 0:3f87ec23c3cf | 15 | motor[2] = new kohiMD(PA_9); |
piroro4560 | 0:3f87ec23c3cf | 16 | motor[3] = new kohiMD(PB_10); |
piroro4560 | 1:6a5065829cfc | 17 | spin.setInputLimits(-180, 180); |
piroro4560 | 1:6a5065829cfc | 18 | spin.setOutputLimits(-0.3,0.3); |
piroro4560 | 1:6a5065829cfc | 19 | spin.setBias(0); |
piroro4560 | 1:6a5065829cfc | 20 | spin.setMode(1); |
piroro4560 | 1:6a5065829cfc | 21 | spin.setSetPoint(0); |
piroro4560 | 1:6a5065829cfc | 22 | shot.setkickperiod(0.5); |
piroro4560 | 1:6a5065829cfc | 23 | shot.setoutputtime(0.01); |
piroro4560 | 1:6a5065829cfc | 24 | ui.beep(1000, 1.0); |
piroro4560 | 0:3f87ec23c3cf | 25 | } |
piroro4560 | 0:3f87ec23c3cf | 26 | |
piroro4560 | 1:6a5065829cfc | 27 | void robot::chaseBall(float ball_theta, float ball_dis, float r) |
piroro4560 | 0:3f87ec23c3cf | 28 | { |
piroro4560 | 1:6a5065829cfc | 29 | theta = ball_theta + ballExtra * ((r > 0) - (r <= 0)); |
piroro4560 | 1:6a5065829cfc | 30 | spin.setProcessValue(r); |
piroro4560 | 1:6a5065829cfc | 31 | omni.computeCircular( ,spin.compute()); |
piroro4560 | 0:3f87ec23c3cf | 32 | } |
piroro4560 | 0:3f87ec23c3cf | 33 | |
piroro4560 | 1:6a5065829cfc | 34 | void robot::lostBall() |
piroro4560 | 0:3f87ec23c3cf | 35 | { |
piroro4560 | 0:3f87ec23c3cf | 36 | |
piroro4560 | 0:3f87ec23c3cf | 37 | } |
piroro4560 | 0:3f87ec23c3cf | 38 | |
piroro4560 | 0:3f87ec23c3cf | 39 | void robot::moveGoal(float r, float goal_size, float goal_) |
piroro4560 | 0:3f87ec23c3cf | 40 | { |
piroro4560 | 0:3f87ec23c3cf | 41 | |
piroro4560 | 0:3f87ec23c3cf | 42 | } |
piroro4560 | 0:3f87ec23c3cf | 43 | |
piroro4560 | 0:3f87ec23c3cf | 44 | void robot::detour() |
piroro4560 | 0:3f87ec23c3cf | 45 | { |
piroro4560 | 0:3f87ec23c3cf | 46 | |
piroro4560 | 0:3f87ec23c3cf | 47 | } |
piroro4560 | 0:3f87ec23c3cf | 48 | |
piroro4560 | 0:3f87ec23c3cf | 49 | void robot::outLine(float r, line) |
piroro4560 | 0:3f87ec23c3cf | 50 | { |
piroro4560 | 0:3f87ec23c3cf | 51 | |
piroro4560 | 0:3f87ec23c3cf | 52 | } |
piroro4560 | 0:3f87ec23c3cf | 53 | |
piroro4560 | 0:3f87ec23c3cf | 54 | |
piroro4560 | 0:3f87ec23c3cf | 55 | |
piroro4560 | 0:3f87ec23c3cf | 56 | void robot::shotBall() |
piroro4560 | 0:3f87ec23c3cf | 57 | { |
piroro4560 | 1:6a5065829cfc | 58 | shot.outPut; |
piroro4560 | 1:6a5065829cfc | 59 | } |
piroro4560 | 1:6a5065829cfc | 60 | |
piroro4560 | 1:6a5065829cfc | 61 | void test() |
piroro4560 | 1:6a5065829cfc | 62 | { |
piroro4560 | 1:6a5065829cfc | 63 | while(1) { |
piroro4560 | 1:6a5065829cfc | 64 | for (int i=0; i<4; i++) { |
piroro4560 | 1:6a5065829cfc | 65 | motor[i]->setSpeed(0.2); |
piroro4560 | 1:6a5065829cfc | 66 | wait(1.0); |
piroro4560 | 1:6a5065829cfc | 67 | } |
piroro4560 | 1:6a5065829cfc | 68 | for (int i=0; i<4; i++) { |
piroro4560 | 1:6a5065829cfc | 69 | motor[i]->setSpeed(-0.2); |
piroro4560 | 1:6a5065829cfc | 70 | wait(1.0); |
piroro4560 | 1:6a5065829cfc | 71 | } |
piroro4560 | 1:6a5065829cfc | 72 | |
piroro4560 | 1:6a5065829cfc | 73 | } |
piroro4560 | 0:3f87ec23c3cf | 74 | } |