File content as of revision 22:682cc376da6f:

#include "bot.h"

Bot::Bot() :
    PIDC(),
    pad1(XBee1TX, XBee1RX, ADDR1),
    /* pad2(XBee2TX, XBee2RX, ADDR2),*/
    motor(MDSDA, MDSCL)
{
  motor.goXY(0, 0, 0);
  motor.moveRightTentacle(0);
  motor.moveLeftTentacle(0);
  motor.windUp(0);
}

void Bot::confirmAll()
{
    receiveSuccessed = pad1.receiveState();
    if(!receiveSuccessed) {
        motor.goXY(0, 0, 0);
        motor.moveLeftTentacle(0);
        motor.moveRightTentacle(0);
        motor.windUp(0);
    }
    //pad2.receiveState();
    //PIDC::confirm();
    //if(pad1.getNorm(1) > 0.5) PIDC::setSetPoint(pad1.getRadian(1) * (180.0 / M_PI));
}

void Bot::controllDrive()
{
    //motor.goXY(pad1.getStick(0),pad1.getStick(1), PIDC::co);
    if(receiveSuccessed) {
        if(pad1.getNorm(1) > 0.5) {
            PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
            PIDC::confirm();
        }
        motor.goXY(
            pad1.getStick(0) / 2.0,
            -pad1.getStick(1) /2.0,
            PIDC::calculationResult
        );
    } else {
        motor.goXY(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(receiveSuccessed) {
        motor.goCircular(
            pad1.getNorm(0) / 2.0,
            pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
            moment
        );
    } else {
        motor.goXY(0, 0, 0);
    }
    beforestick = pad1.getStick(2);
}

void Bot::controllMech()
{
    if(receiveSuccessed){
      if(!pad1.getButton1(0)) motor.moveRightTentacle(TENTACLE_MAX_SPEED);
      if(!pad1.getButton1(1)) motor.moveRightTentacle(-TENTACLE_MAX_SPEED);
      if(pad1.getButton1(0) && pad1.getButton1(1)) motor.moveRightTentacle(0);

      if(!pad1.getButton1(2)) motor.moveLeftTentacle(TENTACLE_MAX_SPEED);
      if(!pad1.getButton1(3)) motor.moveLeftTentacle(-TENTACLE_MAX_SPEED);
      if(pad1.getButton1(2) && pad1.getButton1(3)) motor.moveLeftTentacle(0);

      if(!pad1.getButton1(4)) motor.windUp(WIND_UP_SPEED);
      if(!pad1.getButton1(5)) motor.windUp(-WIND_UP_SPEED);
      if(pad1.getButton1(4) && pad1.getButton1(5)) motor.windUp(0);

    } else {
      motor.moveLeftTentacle(0);
      motor.moveRightTentacle(0);
      motor.windUp(0);
    }

}

void Bot::calibrate(){

}