File content as of revision 26:7258d5ad0bff:

#include "bot.h"

Bot::Bot() :
    pad1(XBee1TX, XBee1RX, ADDR1),
    pad2(XBee2TX, XBee2RX, ADDR2),
    RS485(MDTX, MDRX, 38400),
    RS485Controller(PWM4),
    powerSwitch(MDstop),
    quadOmni(&RS485Controller, &RS485),
    tentacle(&RS485Controller, &RS485),
    nishijo(&RS485Controller, &RS485),
    nishijoSword(&RS485Controller, &RS485),
    receiveSuccessed1(0),
    receiveSuccessed2(0),
    frontDegree(0),
    led({DebugLED3, DebugLED4, DebugLED5}),
    debugSerial(USBTX, USBRX, 230400)
{
    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::confirmPad1()
{
    receiveSuccessed1 = pad1.receiveState();
    if(!receiveSuccessed1) {
        quadOmni.moveXY(0, 0, 0);
    }
}


void Bot::confirmPad2()
{
    receiveSuccessed2 = pad2.receiveState();
    if(!receiveSuccessed2){
      tentacle.stop();
      nishijo.stop();
      nishijoSword.stop();
    }
}

void Bot::drive()
{
    if(receiveSuccessed1) {
        quadOmni.moveXY(
            pad1.getStick(2),
            -pad1.getStick(3),
            0,
            0,
            - pad1.getStick(0) / 3
        );
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
}

void Bot::controllDrive()
{
    if(receiveSuccessed1) {
        /*if(pad1.getNorm(0) > 0.5) {
            plane.setSetPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
            //plane.confirm();
        }*/
        plane.confirm();
        if(pad1.getStick(0))  plane.resetOffset();
        quadOmni.moveXY(
            pad1.getStick(2),
            -pad1.getStick(3),
            0,
            0,
            - pad1.getStick(0) / 2.0
            - plane.getCalculationResult()
        );
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
}

void Bot::controllDrive2()
{
    plane.confirm();
    axis.confirm();
    float moment = 0;
    static float beforestick = pad1.getStick(2);

    if(!pad1.getButton2(2)) {
        axis.resetOffset();
    }

    if((beforestick >= 0.5 && pad1.getStick(2) < 0.5) || (beforestick <= -0.5 && pad1.getStick(2) > -0.5)) {
        plane.setSetPoint(0.0);
        plane.resetOffset();
    }

    if(pad1.getStick(2) > 0.5 || pad1.getStick(2) < -0.5) {
        moment = pad1.getStick(2) / 2.0;
    } else  {
        moment = plane.getCalculationResult();
    }

    if(receiveSuccessed1) {
        led[1] = !led[1];
        quadOmni.moveCircular(
            pad1.getNorm(0),
            pad1.getRadian(0) - axis.getCurrentDegree() / 10.0 * (M_PI / 180.0) + M_PI,
            0.5,
            0.5,
            -moment
        );
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
    beforestick = pad1.getStick(2);
}

void Bot::controllDrive3()
{
    static int rollR = 0;
    static int rollL = 0;
    static int mode = 1;
    if(receiveSuccessed1) {
        if(rollR && !pad1.getButton2(2)) {
            frontDegree += ADJUST_DEGREE;
        }
        rollR = pad1.getButton2(2);

        if(rollL && !pad1.getButton2(0)) {
            frontDegree -= ADJUST_DEGREE;
        }
        rollL = pad1.getButton2(0);

        if(!pad1.getButton2(4)) {
            mode = 1;
        }

        if(!pad1.getButton2(5)) {
            mode = 2;
        }

        if(mode == 1) {
            plane.setPoint(frontDegree);
            plane.confirm();

            quadOmni.moveXY(
                pad1.getStick(0),
                -pad1.getStick(1),
                0.5,
                0.5,
                -plane.getCalculationResult()
            );
        }
        if(mode == 2) {
            plane.setPoint(90.0 + frontDegree);
            plane.confirm();

            quadOmni.moveXY(
                -pad1.getStick(1),
                -pad1.getStick(0),
                0.5,
                0.5,
                -plane.getCalculationResult()
            );
        }
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
}

void Bot::controllMech()
{

    if(receiveSuccessed2) {

        tentacle.leftMove(pad2.getStick(3));
        tentacle.rightMove(pad2.getStick(1));

        if (!pad2.getButton2(0)) nishijoSword.move(-WIND_UP_SPEED);
        if (!pad2.getButton2(1)) nishijoSword.move(WIND_UP_SPEED);
        if(pad2.getButton2(0) && pad2.getButton2(1))  nishijoSword.move(0);

        if (!pad2.getButton2(2)) nishijo.move(-SWORD_SPEED);
        if (!pad2.getButton2(3)) nishijo.move(SWORD_SPEED);
        if(pad2.getButton2(2) && pad2.getButton2(3))  nishijo.move(0);

    } else {
      tentacle.stop();
      nishijoSword.stop();
      nishijo.stop();
    }
}


void Bot::calibrate()
{
    if(receiveSuccessed1 &&
        !pad1.getButton2(0) &&
        !pad1.getButton2(1) &&
        !pad1.getButton2(2) &&
        !pad1.getButton2(3)
      ) {
        t.start();
        t.reset();
        plane.calibration(HMC6352_ENTER_CALIB);
        while(t.read() < 5.0) {
            quadOmni.moveXY(0, 0, 0.4);
            nishijo.stop();
            nishijoSword.stop();
            tentacle.stop();
        }
        t.stop();
        quadOmni.moveXY(0, 0, 0);
        plane.calibration(HMC6352_EXIT_CALIB);
    }
}


void Bot::checkConnection()
{
  if(receiveSuccessed1) {
    debugSerial.printf("ON ");
  } else {
    debugSerial.printf("OFF ");
  }
  if(receiveSuccessed2) {
    debugSerial.printf("ON\r\n");
  } else {
    debugSerial.printf("OFF\r\n");
  }
}

void Bot::debugCompass()
{
  if(receiveSuccessed1) {
    debugSerial.printf("%f %f %f\r\n",plane.getRawDegree()/10.0 ,-plane.getCalculationResult(),-pad1.getStick(0));
  } else {
    debugSerial.printf("\r\n");
  }
}