File content as of revision 3:369d9ee17e84:

#include "bot.h"

Bot::Bot() :
    PIDC(), pad(XBee1TX, XBee1RX, ADDR), motor(MDSDA, MDSCL, solenoidPin)
{
    motor.goXY(0, 0, 0);
    motor.moveSlider(0);
    motor.destroy(0);
    motor.swing(0);
    motor.shakeHead(0);
}

void Bot::confirmAll()
{
    suc = pad.receiveState();
    PIDC::confirm();
    if(!suc) {
        motor.goXY(0, 0, 0);
        motor.moveSlider(0);
        motor.destroy(0);
        motor.swing(0);
        motor.shakeHead(0);
    }
}

void Bot::controllDrive()
{
    if(suc) {
        if(pad.getNorm(1) > 0.5) {
            PIDC::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
            PIDC::confirm();
        }
        motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) /2.0, PIDC::co);
    } else {
        motor.goXY(0, 0, 0);
    }
}

void Bot::controllDrive2()
{
    static float moment = 0;
    static float beforestick = pad.getStick(2);

    if((beforestick >= 0.1 && pad.getStick(2) < 0.1) || (beforestick <= -0.1 && pad.getStick(2) > -0.1)) {
        PIDC::setSetPoint(PIDC::getDegree());
    }

    if(pad.getStick(2) > 0.1 && pad.getStick(2) < -0.1) {
        moment = pad.getStick(2) / 2.0;
        PIDC::confirm();
    } else {
        PIDC::confirm();
        moment = PIDC::co;
    }

    if(suc) {
        motor.goXY(pad.getStick(0) / 2.0, pad.getStick(1) / 2.0, moment);
    } else {
        motor.goXY(0, 0, 0);
    }
    beforestick = pad.getStick(2);
}

void Bot::controllMech()
{
    if(suc) {
//        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(0.2);
        if(!pad.getButton1(6)) motor.swing(-0.2);
        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(1)) motor.release();
   } else {
       motor.moveSlider(0);
       motor.destroy(0);
       motor.swing(0);
       motor.shakeHead(0);
   }
}


void Bot::calibrate()
{
    if(suc && !pad.getButton2(0) && !pad.getButton2(1)) {
        PIDC::calibration(HMC6352_ENTER_CALIB);
        motor.goXY(0, 0, 0.4);
        wait(5.0);
        motor.goXY(0, 0, 0);
        PIDC::calibration(HMC6352_EXIT_CALIB);
    }
}