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:
- 24:9a4ca5442717
- Parent:
- 23:37bb9afe9fdc
- Child:
- 25:d199d621ecca
File content as of revision 24:9a4ca5442717:
#include "bot.h" Bot::Bot() : PIDC(), pad1(XBee1TX, XBee1RX, ADDR1), //pad2(XBee2TX, XBee2RX, ADDR2), RS485(MDTX, MDRX, 38400), RS485Controller(PWM2), 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}), test(PWM1), debugSerial(USBTX, USBRX, 115200) { test = 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() { 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::confirm(); PIDC::renewAngle(); test = !test; quadOmni.moveXY( pad1.getStick(0) * 0.8, -pad1.getStick(1) * 0.8, -pad1.getStick(2) * 0.4 //PIDC::calculationResult ); /*quadOmni.moveCircular( 0.2, 0, 0 );*/ } 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(2) && !pad1.getButton1(3) && !pad1.getButton1(4) && !pad1.getButton1(5)) powerSwitch = 0; //else powerSwitch = 1; } else { tentacle.stop(); //powerSwitch = 0; } /*if(receiveSuccessed2){ }*/ } void Bot::calibrate() { } void Bot::debugPrint() { if(receiveSuccessed1) { //debugSerial.printf("%f %f %f %f \r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3)); //debugSerial.printf("%f %f %f\r\n", PIDC::beforeDegree, PIDC::rawDegree, PIDC::calculationResult); } else { debugSerial.printf("\r\n"); } }