NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
bot/bot.cpp@22:682cc376da6f, 2017-09-07 (annotated)
- Committer:
- number_key
- Date:
- Thu Sep 07 06:26:53 2017 +0000
- Revision:
- 22:682cc376da6f
- Parent:
- 21:9c1061982b16
- Child:
- 23:37bb9afe9fdc
save
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 0:757e9800c103 | 1 | #include "bot.h" |
UCHITAKE | 0:757e9800c103 | 2 | |
uchitake |
18:6f52f4b1086a | 3 | Bot::Bot() : |
number_key | 22:682cc376da6f | 4 | PIDC(), |
number_key | 22:682cc376da6f | 5 | pad1(XBee1TX, XBee1RX, ADDR1), |
number_key | 22:682cc376da6f | 6 | /* pad2(XBee2TX, XBee2RX, ADDR2),*/ |
number_key | 22:682cc376da6f | 7 | motor(MDSDA, MDSCL) |
UCHITAKE | 0:757e9800c103 | 8 | { |
number_key | 22:682cc376da6f | 9 | motor.goXY(0, 0, 0); |
number_key | 22:682cc376da6f | 10 | motor.moveRightTentacle(0); |
number_key | 22:682cc376da6f | 11 | motor.moveLeftTentacle(0); |
number_key | 22:682cc376da6f | 12 | motor.windUp(0); |
UCHITAKE | 1:269914e0aa07 | 13 | } |
UCHITAKE | 1:269914e0aa07 | 14 | |
UCHITAKE | 1:269914e0aa07 | 15 | void Bot::confirmAll() |
UCHITAKE | 1:269914e0aa07 | 16 | { |
number_key | 22:682cc376da6f | 17 | receiveSuccessed = pad1.receiveState(); |
number_key | 22:682cc376da6f | 18 | if(!receiveSuccessed) { |
number_key | 22:682cc376da6f | 19 | motor.goXY(0, 0, 0); |
number_key | 22:682cc376da6f | 20 | motor.moveLeftTentacle(0); |
number_key | 22:682cc376da6f | 21 | motor.moveRightTentacle(0); |
number_key | 22:682cc376da6f | 22 | motor.windUp(0); |
number_key | 22:682cc376da6f | 23 | } |
number_key | 22:682cc376da6f | 24 | //pad2.receiveState(); |
number_key | 22:682cc376da6f | 25 | //PIDC::confirm(); |
number_key | 22:682cc376da6f | 26 | //if(pad1.getNorm(1) > 0.5) PIDC::setSetPoint(pad1.getRadian(1) * (180.0 / M_PI)); |
UCHITAKE | 1:269914e0aa07 | 27 | } |
UCHITAKE | 1:269914e0aa07 | 28 | |
UCHITAKE | 1:269914e0aa07 | 29 | void Bot::controllDrive() |
UCHITAKE | 1:269914e0aa07 | 30 | { |
number_key | 22:682cc376da6f | 31 | //motor.goXY(pad1.getStick(0),pad1.getStick(1), PIDC::co); |
number_key | 22:682cc376da6f | 32 | if(receiveSuccessed) { |
number_key | 22:682cc376da6f | 33 | if(pad1.getNorm(1) > 0.5) { |
number_key | 22:682cc376da6f | 34 | PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI)); |
number_key | 22:682cc376da6f | 35 | PIDC::confirm(); |
number_key | 22:682cc376da6f | 36 | } |
number_key | 22:682cc376da6f | 37 | motor.goXY( |
number_key | 22:682cc376da6f | 38 | pad1.getStick(0) / 2.0, |
number_key | 22:682cc376da6f | 39 | -pad1.getStick(1) /2.0, |
number_key | 22:682cc376da6f | 40 | PIDC::calculationResult |
number_key | 22:682cc376da6f | 41 | ); |
number_key | 22:682cc376da6f | 42 | } else { |
number_key | 22:682cc376da6f | 43 | motor.goXY(0, 0, 0); |
number_key | 22:682cc376da6f | 44 | } |
number_key | 22:682cc376da6f | 45 | } |
number_key | 22:682cc376da6f | 46 | |
number_key | 22:682cc376da6f | 47 | void Bot::controllDrive2() |
number_key | 22:682cc376da6f | 48 | { |
number_key | 22:682cc376da6f | 49 | float moment = 0; |
number_key | 22:682cc376da6f | 50 | static float beforestick = pad1.getStick(2); |
number_key | 22:682cc376da6f | 51 | |
number_key | 22:682cc376da6f | 52 | if(!pad1.getButton2(2)) { |
number_key | 22:682cc376da6f | 53 | PIDC::resetPlaneOffset(); |
number_key | 22:682cc376da6f | 54 | } |
number_key | 22:682cc376da6f | 55 | |
number_key | 22:682cc376da6f | 56 | if((beforestick >= 0.5 && pad1.getStick(2) < 0.5) || (beforestick <= -0.5 && pad1.getStick(2) > -0.5)) { |
number_key | 22:682cc376da6f | 57 | PIDC::PID::setSetPoint(0.0); |
number_key | 22:682cc376da6f | 58 | PIDC::resetAxisOffset(); |
number_key | 22:682cc376da6f | 59 | } |
number_key | 22:682cc376da6f | 60 | |
number_key | 22:682cc376da6f | 61 | if(pad1.getStick(2) > 0.5 || pad1.getStick(2) < -0.5) { |
number_key | 22:682cc376da6f | 62 | moment = pad1.getStick(2) / 4.0; |
number_key | 22:682cc376da6f | 63 | PIDC::confirm(); |
number_key | 22:682cc376da6f | 64 | } |
number_key | 22:682cc376da6f | 65 | if(fabs(pad1.getStick(2)) < 0.5) { |
number_key | 22:682cc376da6f | 66 | PIDC::confirm(); |
number_key | 22:682cc376da6f | 67 | moment = PIDC::calculationResult; |
number_key | 22:682cc376da6f | 68 | } |
number_key | 22:682cc376da6f | 69 | |
number_key | 22:682cc376da6f | 70 | if(receiveSuccessed) { |
number_key | 22:682cc376da6f | 71 | motor.goCircular( |
number_key | 22:682cc376da6f | 72 | pad1.getNorm(0) / 2.0, |
number_key | 22:682cc376da6f | 73 | pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI, |
number_key | 22:682cc376da6f | 74 | moment |
number_key | 22:682cc376da6f | 75 | ); |
number_key | 22:682cc376da6f | 76 | } else { |
number_key | 22:682cc376da6f | 77 | motor.goXY(0, 0, 0); |
number_key | 22:682cc376da6f | 78 | } |
number_key | 22:682cc376da6f | 79 | beforestick = pad1.getStick(2); |
UCHITAKE | 1:269914e0aa07 | 80 | } |
UCHITAKE | 1:269914e0aa07 | 81 | |
UCHITAKE | 1:269914e0aa07 | 82 | void Bot::controllMech() |
UCHITAKE | 1:269914e0aa07 | 83 | { |
number_key | 22:682cc376da6f | 84 | if(receiveSuccessed){ |
number_key | 22:682cc376da6f | 85 | if(!pad1.getButton1(0)) motor.moveRightTentacle(TENTACLE_MAX_SPEED); |
number_key | 22:682cc376da6f | 86 | if(!pad1.getButton1(1)) motor.moveRightTentacle(-TENTACLE_MAX_SPEED); |
number_key | 22:682cc376da6f | 87 | if(pad1.getButton1(0) && pad1.getButton1(1)) motor.moveRightTentacle(0); |
number_key | 22:682cc376da6f | 88 | |
number_key | 22:682cc376da6f | 89 | if(!pad1.getButton1(2)) motor.moveLeftTentacle(TENTACLE_MAX_SPEED); |
number_key | 22:682cc376da6f | 90 | if(!pad1.getButton1(3)) motor.moveLeftTentacle(-TENTACLE_MAX_SPEED); |
number_key | 22:682cc376da6f | 91 | if(pad1.getButton1(2) && pad1.getButton1(3)) motor.moveLeftTentacle(0); |
number_key |
21:9c1061982b16 | 92 | |
number_key | 22:682cc376da6f | 93 | if(!pad1.getButton1(4)) motor.windUp(WIND_UP_SPEED); |
number_key | 22:682cc376da6f | 94 | if(!pad1.getButton1(5)) motor.windUp(-WIND_UP_SPEED); |
number_key | 22:682cc376da6f | 95 | if(pad1.getButton1(4) && pad1.getButton1(5)) motor.windUp(0); |
UCHITAKE | 1:269914e0aa07 | 96 | |
number_key | 22:682cc376da6f | 97 | } else { |
number_key | 22:682cc376da6f | 98 | motor.moveLeftTentacle(0); |
number_key | 22:682cc376da6f | 99 | motor.moveRightTentacle(0); |
number_key | 22:682cc376da6f | 100 | motor.windUp(0); |
number_key | 22:682cc376da6f | 101 | } |
uchitake |
15:9a2dce34b660 | 102 | |
uchitake |
15:9a2dce34b660 | 103 | } |
number_key | 22:682cc376da6f | 104 | |
number_key | 22:682cc376da6f | 105 | void Bot::calibrate(){ |
number_key | 22:682cc376da6f | 106 | |
number_key | 22:682cc376da6f | 107 | } |