NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated NHK2017_octopus

NHK2017 octopus robot

Dependencies:   2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel

Fork of KANI2017v2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp

Committer:
uchitake
Date:
2017-08-30
Revision:
18:6f52f4b1086a
Parent:
16:86958857c739
Child:
20:477c5d039e93

File content as of revision 18:6f52f4b1086a:

#include "bot.h"

Bot::Bot() :
PIDC(), pad(XBee1TX, XBee1RX, ADDR), motor(MDSDA, MDSCL, solenoidPin)
{
}

void Bot::confirmAll()
{
    pad.receiveState();
    PIDC::confirm();
    if(pad.getNorm(1) > 0.5) PIDC::setSetPoint(pad.getRadian(1) * (180.0 / M_PI));
}

void Bot::controllDrive()
{
    motor.goXY(pad.getStick(0),pad.getStick(1), PIDC::co);
}

void Bot::controllMech()
{
    if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
    if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
    if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);

    if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
    if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
    if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);

    if(!pad.getButton1(5)) motor.swing(ARM_MAX_SPEED);
    if(!pad.getButton1(6)) motor.swing(-ARM_MAX_SPEED);
    if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);

    if(!pad.getButton1(2)) {
        motor.destroy(DESTROY_MAX_SPEED);
    } else {
        motor.destroy(0);
    }

    if(!pad.getButton2(0)) motor.release();
}