![](/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
bot/bot.cpp
- Committer:
- number_key
- Date:
- 2017-09-13
- Revision:
- 23:37bb9afe9fdc
- Parent:
- 22:682cc376da6f
- Child:
- 24:9a4ca5442717
File content as of revision 23:37bb9afe9fdc:
#include "bot.h" Bot::Bot() : PIDC(), pad1(XBee1TX, XBee1RX, ADDR1), //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) { 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() { receiveSuccessed1 = pad1.receiveState(); //receiveSuccessed2 = pad2.receiveState(); if(!receiveSuccessed1) { quadOmni.moveXY(0, 0, 0); tentacle.stop(); } } void Bot::controllDrive() { if(receiveSuccessed1) { if(pad1.getNorm(1) > 0.5) { PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI)); PIDC::confirm(); } PIDC::renewAngle(); quadOmni.moveXY( pad1.getStick(0) / 2.0, -pad1.getStick(1) /2.0, PIDC::calculationResult ); } else { quadOmni.moveXY(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(receiveSuccessed1) { quadOmni.moveCircular( pad1.getNorm(0) / 2.0, pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI, moment ); } else { quadOmni.moveXY(0, 0, 0); } beforestick = pad1.getStick(2); } void Bot::controllMech() { 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.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(0)); powerSwitch = 0; } else { tentacle.stop(); //powerSwitch = 0; } /*if(receiveSuccessed2){ }*/ } 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"); } }