File content as of revision 37:6b6616008e78:

#include "bot.h"

Bot::Bot() :
    pad(XBee2TX, XBee2RX, ADDR),
    RS485(MDTX, MDRX, 38400),
    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})
            }),
    receiveSuccessed(0),
    frontDegree(0),
    led({DebugLED3, DebugLED4, DebugLED5}),
    debugSerial(USBTX, USBRX, 115200)
{
    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(3) && !pad.getButton2(0)) {
        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) {
        debugSerial.printf("%d\n\r", plane.getRawDegree());
        led[1] = !led[1];
        if(pad.getNorm(1) > 0.5) {
            plane.setSetPoint((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 // PIDC::calculationResult
                );
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
}

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

    if(!pad.getButton1(6)) {
        axis.resetOffset();
        debugSerial.printf("Force RESeT\n\r");
    }

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

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

    if(receiveSuccessed) {
        led[1] = !led[1];
        quadOmni.moveCircular(
                pad.getNorm(0),
                pad.getRadian(0) - axis.getCurrentDegree() / 10.0 * (M_PI / 180.0) + M_PI,
                0.2,
                0.2,
                -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::controllMech()
{
    if(receiveSuccessed) {
        if(!pad.getButton1(2)) {
            debugSerial.printf("ROLL+\n\r");
            armMotor[ROLL].setSpeed(1.0);
        } else if(!pad.getButton1(4)) {
            debugSerial.printf("ROLL-\n\r");
            armMotor[ROLL].setSpeed(-1.0);
        }
        if(pad.getButton1(2) && pad.getButton1(4)) {
            armMotor[ROLL].setSpeed(0.0);
        }

        if(!pad.getButton1(0)) {
            debugSerial.printf("FUKUDA\n\r");
            armMotor[SWORD].setSpeed(1.0);
        }
        if(pad.getButton1(0)) armMotor[SWORD].setSpeed(0.0);

        slider.slide(pad.getStick(3));

        if(!pad.getButton2(1)) {
            debugSerial.printf("DESTROYYY\n\r");
            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);
        }
    }
}


void Bot::calibrate()
{
    if(receiveSuccessed &&
            !pad.getButton2(0) &&
            !pad.getButton2(1)
      ) {
        t.start();
        t.reset();
        plane.calibration(HMC6352_ENTER_CALIB);
        while(t.read() < 5.0) {
            quadOmni.moveXY(0, 0, 0.4);
            slider.slide(0);
            for(int i = 0; i < 3; i++) {
                armMotor[i].setSpeed(0);
            }
        }
        t.stop();
        quadOmni.moveXY(0, 0, 0);
        plane.calibration(HMC6352_EXIT_CALIB);
    }
}