![](/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:
- 23:37bb9afe9fdc
- Parent:
- 22:682cc376da6f
- Child:
- 24:9a4ca5442717
diff -r 682cc376da6f -r 37bb9afe9fdc bot/bot.cpp --- a/bot/bot.cpp Thu Sep 07 06:26:53 2017 +0000 +++ b/bot/bot.cpp Wed Sep 13 14:26:47 2017 +0900 @@ -3,44 +3,63 @@ Bot::Bot() : PIDC(), pad1(XBee1TX, XBee1RX, ADDR1), - /* pad2(XBee2TX, XBee2RX, ADDR2),*/ - motor(MDSDA, MDSCL) + //pad2(XBee2TX, XBee2RX, ADDR2), + RS485(MDTX, MDRX, 38400), + RS485Controller(PWM1), + powerSwitch(MDstop), + quadOmni(&RS485Controller, &RS485), + tentacle(&RS485Controller, &RS485), + nishijo(&RS485Controller, &RS485), + nishijoSword(&RS485Controller, &RS485), + receiveSuccessed1(0), + //receiveSuccessed2(0), + led({DebugLED1, DebugLED2, DebugLED3, DebugLED4, + DebugLED5, DebugLED6, DebugLED7, DebugLED8}), + debugSerial(USBTX, USBRX, 115200) { - motor.goXY(0, 0, 0); - motor.moveRightTentacle(0); - motor.moveLeftTentacle(0); - motor.windUp(0); + + + quadOmni.moveXY(0, 0, 0); + + powerSwitch = 1; + + for(int i = 0; i < 8; i++) { + led[i] = 1; + wait(0.1); + led[i] = 0; + } + for(int i = 0; i < 8; i++) { + led[i] = 1; + wait(0.1); + led[i] = 0; + } } void Bot::confirmAll() { - receiveSuccessed = pad1.receiveState(); - if(!receiveSuccessed) { - motor.goXY(0, 0, 0); - motor.moveLeftTentacle(0); - motor.moveRightTentacle(0); - motor.windUp(0); + receiveSuccessed1 = pad1.receiveState(); + //receiveSuccessed2 = pad2.receiveState(); + if(!receiveSuccessed1) { + quadOmni.moveXY(0, 0, 0); + tentacle.stop(); } - //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); - if(receiveSuccessed) { + if(receiveSuccessed1) { if(pad1.getNorm(1) > 0.5) { PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI)); PIDC::confirm(); } - motor.goXY( + PIDC::renewAngle(); + quadOmni.moveXY( pad1.getStick(0) / 2.0, -pad1.getStick(1) /2.0, PIDC::calculationResult ); } else { - motor.goXY(0, 0, 0); + quadOmni.moveXY(0, 0, 0); } } @@ -67,41 +86,51 @@ moment = PIDC::calculationResult; } - if(receiveSuccessed) { - motor.goCircular( + if(receiveSuccessed1) { + quadOmni.moveCircular( 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); + quadOmni.moveXY(0, 0, 0); } beforestick = pad1.getStick(2); } void Bot::controllMech() { - 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(receiveSuccessed1) { + if(!pad1.getButton2(2)) tentacle.rightMove(TENTACLE_SPEED); + if(!pad1.getButton2(3)) tentacle.rightMove(-TENTACLE_SPEED); + if(pad1.getButton2(2) && pad1.getButton2(3)) tentacle.rightMove(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.getButton2(0)) tentacle.leftMove(TENTACLE_SPEED); + if(!pad1.getButton2(1)) tentacle.leftMove(-TENTACLE_SPEED); + if(pad1.getButton2(0) && pad1.getButton2(1)) tentacle.leftMove(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); - + //if(!pad1.getButton1(0)); powerSwitch = 0; } else { - motor.moveLeftTentacle(0); - motor.moveRightTentacle(0); - motor.windUp(0); + tentacle.stop(); + //powerSwitch = 0; } + /*if(receiveSuccessed2){ + + }*/ +} + + +void Bot::calibrate() +{ } -void Bot::calibrate(){ - +void Bot::debugPrint() +{ + if(receiveSuccessed1) { + //debugSerial.printf("%f %f %f %f ,%f\r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3)); + debugSerial.printf("%f\r\n",PIDC::angle); + } else { + debugSerial.printf("\r\n"); + } }