![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
Diff: bot/bot.cpp
- Revision:
- 22:682cc376da6f
- Parent:
- 21:9c1061982b16
- Child:
- 23:37bb9afe9fdc
diff -r 9c1061982b16 -r 682cc376da6f bot/bot.cpp --- a/bot/bot.cpp Tue Sep 05 14:07:18 2017 +0900 +++ b/bot/bot.cpp Thu Sep 07 06:26:53 2017 +0000 @@ -1,34 +1,107 @@ #include "bot.h" Bot::Bot() : -PIDC(), pad1(XBee1TX, XBee1RX, ADDR1), pad2(XBee2TX, XBee2RX, ADDR2), motor(MDSDA, MDSCL) + PIDC(), + pad1(XBee1TX, XBee1RX, ADDR1), + /* pad2(XBee2TX, XBee2RX, ADDR2),*/ + motor(MDSDA, MDSCL) { + motor.goXY(0, 0, 0); + motor.moveRightTentacle(0); + motor.moveLeftTentacle(0); + motor.windUp(0); } void Bot::confirmAll() { - pad1.receiveState(); - pad2.receiveState(); - PIDC::confirm(); - if(pad1.getNorm(1) > 0.5) PIDC::setSetPoint(pad1.getRadian(1) * (180.0 / M_PI)); + receiveSuccessed = pad1.receiveState(); + if(!receiveSuccessed) { + motor.goXY(0, 0, 0); + motor.moveLeftTentacle(0); + motor.moveRightTentacle(0); + motor.windUp(0); + } + //pad2.receiveState(); + //PIDC::confirm(); + //if(pad1.getNorm(1) > 0.5) PIDC::setSetPoint(pad1.getRadian(1) * (180.0 / M_PI)); } void Bot::controllDrive() { - motor.goXY(pad1.getStick(0),pad1.getStick(1), PIDC::co); + //motor.goXY(pad1.getStick(0),pad1.getStick(1), PIDC::co); + if(receiveSuccessed) { + if(pad1.getNorm(1) > 0.5) { + PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI)); + PIDC::confirm(); + } + motor.goXY( + pad1.getStick(0) / 2.0, + -pad1.getStick(1) /2.0, + PIDC::calculationResult + ); + } else { + motor.goXY(0, 0, 0); + } +} + +void Bot::controllDrive2() +{ + float moment = 0; + static float beforestick = pad1.getStick(2); + + if(!pad1.getButton2(2)) { + PIDC::resetPlaneOffset(); + } + + if((beforestick >= 0.5 && pad1.getStick(2) < 0.5) || (beforestick <= -0.5 && pad1.getStick(2) > -0.5)) { + PIDC::PID::setSetPoint(0.0); + PIDC::resetAxisOffset(); + } + + if(pad1.getStick(2) > 0.5 || pad1.getStick(2) < -0.5) { + moment = pad1.getStick(2) / 4.0; + PIDC::confirm(); + } + if(fabs(pad1.getStick(2)) < 0.5) { + PIDC::confirm(); + moment = PIDC::calculationResult; + } + + if(receiveSuccessed) { + motor.goCircular( + pad1.getNorm(0) / 2.0, + pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI, + moment + ); + } else { + motor.goXY(0, 0, 0); + } + beforestick = pad1.getStick(2); } void Bot::controllMech() { - if(!pad1.getButton1(0)) motor.moveRightTentacle(TENTACLE_MAX_SPEED); - if(!pad1.getButton1(1)) motor.moveRightTentacle(-TENTACLE_MAX_SPEED); - if(pad1.getButton1(0) && pad1.getButton1(1)) motor.moveRightTentacle(0); + if(receiveSuccessed){ + if(!pad1.getButton1(0)) motor.moveRightTentacle(TENTACLE_MAX_SPEED); + if(!pad1.getButton1(1)) motor.moveRightTentacle(-TENTACLE_MAX_SPEED); + if(pad1.getButton1(0) && pad1.getButton1(1)) motor.moveRightTentacle(0); + + if(!pad1.getButton1(2)) motor.moveLeftTentacle(TENTACLE_MAX_SPEED); + if(!pad1.getButton1(3)) motor.moveLeftTentacle(-TENTACLE_MAX_SPEED); + if(pad1.getButton1(2) && pad1.getButton1(3)) motor.moveLeftTentacle(0); - if(!pad1.getButton1(2)) motor.moveLeftTentacle(TENTACLE_MAX_SPEED); - if(!pad1.getButton1(3)) motor.moveLeftTentacle(-TENTACLE_MAX_SPEED); - if(pad1.getButton1(2) && pad1.getButton1(3)) motor.moveLeftTentacle(0); + if(!pad1.getButton1(4)) motor.windUp(WIND_UP_SPEED); + if(!pad1.getButton1(5)) motor.windUp(-WIND_UP_SPEED); + if(pad1.getButton1(4) && pad1.getButton1(5)) motor.windUp(0); - - + } else { + motor.moveLeftTentacle(0); + motor.moveRightTentacle(0); + motor.windUp(0); + } } + +void Bot::calibrate(){ + +}