kani
Dependencies: 2017NHKpin_config FEP omni_wheel PID R1307 ikarashiMDC
\ ̄\ / ̄/ /l \ \ / / lヽ | ヽ ヽ | | / / | \ ` ‐ヽ ヽ ● ● / / ‐ / \ __ l | ||___|| / l __ / \ \ / \/ /\| 人__人 |/\ <ズワイガニ //\| |/\\ //\| ケガニ |/\\ / . \_____/ \ ┏┓ ┏━┓┏┓ ┏┓ ┏┓┏┓ ┏┓ ┏┓┗┛ ┏┓ ┗┓┃┗┛ ┏┛┗━┓ ┃┃┃┃ ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ ┗┓┏━┛ ┃┃┗┛ ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛ ┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ ┏┛┃┃┃ ┃┃ ┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ ┗━┛┃┃ ┃┃┏┓ ┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃ ┏┛┃ ┃┃┃┗━━┓ ┗┛┗━━┛┗━━━┛┗┛┗━━┛ ┗━┛ ┗┛┗━━━┛
bot/bot.cpp
- Committer:
- takeuchi
- Date:
- 2017-12-22
- Revision:
- 55:f9e13797024f
- Parent:
- 54:be0b5d603562
File content as of revision 55:f9e13797024f:
#include "bot.h" Bot::Bot() : pad(XBee2TX, XBee2RX, ADDR), RS485(MDTX, MDRX, 115200), RS485Controller(PWM1), powerSwitch(MDstop), quadOmni(&RS485Controller, &RS485), slider(&RS485Controller, &RS485), armMotor({ ikarashiMDC({&RS485Controller, 1, 1, SM, &RS485}), ikarashiMDC({&RS485Controller, 1, 2, SM, &RS485}), ikarashiMDC({&RS485Controller, 1, 3, SM, &RS485}) }), plane(), axis(), receiveSuccessed(0), frontDegree(0), led({DebugLED3, DebugLED4, DebugLED5}), debugSerial(USBTX, USBRX, 115200), faceMode(0) { for(int i = 0; i < 3; i++) { armMotor[i].braking = true; } quadOmni.moveXY(0, 0, 0); powerSwitch = true; for(int i = 0; i < 3; i++) { led[i] = true; wait(0.1); led[i] = false; } } void Bot::confirmAll() { receiveSuccessed = pad.receiveState(); if(!pad.getButton2(0) && !pad.getButton2(1) && !pad.getButton2(2) && !pad.getButton2(3)) { powerSwitch = 0; } led[0] = receiveSuccessed; if(!receiveSuccessed) { quadOmni.moveXY(0, 0, 0); slider.slide(0); for(int i = 0; i < 3; i++) { armMotor[i].setSpeed(0); } } } void Bot::controllDrive() { if(receiveSuccessed) { led[1] = !led[1]; if(pad.getNorm(1) > 0.5) { plane.setPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI)); plane.confirm(); } quadOmni.moveXY( pad.getStick(0), -pad.getStick(1), 0, 0, -pad.getStick(2) / 3.0 ); } else { quadOmni.moveXY(0, 0, 0); } } void Bot::controllDrive2() { plane.confirm(); axis.confirm(); float moment = 0; float norm = 0; static float beforestick = pad.getStick(2); if(!pad.getButton2(4)) { axis.resetOffset(); } if((beforestick >= 0.2 && pad.getStick(2) < 0.2) || (beforestick <= -0.2 && pad.getStick(2) > -0.2)) { plane.setPoint(0.0); plane.resetOffset(); } if(pad.getStick(2) > 0.2 || pad.getStick(2) < -0.2) { moment = pad.getStick(2) / 2.5; } else { moment = plane.getCalculationResult(); } if(pad.getButton2(0)) { norm = pad.getNorm(0) / 2.0; } else { norm = pad.getNorm(0); } if(receiveSuccessed) { led[1] = !led[1]; quadOmni.moveCircular( norm, pad.getRadian(0) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + (M_PI / 2.0) * faceMode, 0.0, 0.0, -moment ); } else { quadOmni.moveXY(0, 0, 0); } beforestick = pad.getStick(2); } void Bot::controllDrive3() { static int rollR = 0; static int rollL = 0; static int mode = 1; if(receiveSuccessed) { if(rollR && !pad.getButton2(2)) { frontDegree += ADJUST_DEGREE; } rollR = pad.getButton2(2); if(rollL && !pad.getButton2(0)) { frontDegree -= ADJUST_DEGREE; } rollL = pad.getButton2(0); if(!pad.getButton2(4)) { mode = 1; } if(!pad.getButton2(5)) { mode = 2; } if(mode == 1) { plane.setPoint(frontDegree); plane.confirm(); quadOmni.moveXY( pad.getStick(0), -pad.getStick(1), 0.5, 0.5, -plane.getCalculationResult() ); } if(mode == 2) { plane.setPoint(90.0 + frontDegree); plane.confirm(); quadOmni.moveXY( -pad.getStick(1), -pad.getStick(0), 0.5, 0.5, -plane.getCalculationResult() ); } } else { quadOmni.moveXY(0, 0, 0); } } void Bot::controllDrive4() { if(receiveSuccessed) { quadOmni.moveXY( pad.getStick(0), pad.getStick(1), 0.0, 0.0, -pad.getStick(2)/2.0 ); } else { quadOmni.moveXY(0, 0, 0); } } void Bot::controllMech() { if(receiveSuccessed) { if(!pad.getButton1(2)) { armMotor[ROLL].setSpeed(1.0); } else if(!pad.getButton1(4)) { armMotor[ROLL].setSpeed(-1.0); } if(pad.getButton1(2) && pad.getButton1(4)) { armMotor[ROLL].setSpeed(0.0); } if(!pad.getButton2(3)) { armMotor[SWORD].setSpeed(1.0); } if(!pad.getButton2(2)) { armMotor[SWORD].setSpeed(-1.0); } if(pad.getButton2(3) && pad.getButton2(2)) armMotor[SWORD].setSpeed(0.0); slider.slide(pad.getStick(3)); if(!pad.getButton2(1)) { armMotor[DESTROY].setSpeed(-1.0); } else if(!pad.getButton1(0)) { armMotor[DESTROY].setSpeed(1.0); } else { armMotor[DESTROY].setSpeed(0.0); } } else { slider.slide(0); for(int i = 0; i < 3; i++) { armMotor[i].setSpeed(0); } } if(!pad.getButton1(3)) faceMode = 2; if(!pad.getButton1(5)) faceMode = 0; } void Bot::calibrate() { }