File content as of revision 9:39be1525dfe0:

#include "bot.h"

Bot::Bot() :
    PIDC(),
    pad(XBee1TX, XBee1RX, ADDR),
    RS485(MDTX, MDRX, 38400),
    RS485Controller(PWM1),
    powerSwitch(PWM2),
    quadOmni(&RS485Controller, &RS485),
    slider(&RS485Controller, &RS485),
    armMotor({
        {&RS485Controller, 1, 1, SM, &RS485},
        {&RS485Controller, 1, 2, SM, &RS485},
        {&RS485Controller, 1, 3, SM, &RS485}
    }),
    receiveSuccessed(0),
    led({DebugLED1, DebugLED2, DebugLED3, DebugLED4,
        DebugLED5, DebugLED6, DebugLED7, DebugLED8}),
    debugSerial(USBTX, USBRX, 115200)
{
    for(int i = 0; i < 3; i++) {
        armMotor[i].braking = true;
    }

    quadOmni.moveXY(0, 0, 0);

    powerSwitch = 1;

    for(int i = 0; i < 8; i++) {
        led[i] = 1;
        wait(0.1);
        led[i] = 0;
    }
}

void Bot::confirmAll()
{
    receiveSuccessed = pad.receiveState();
    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) {
        if(pad.getNorm(1) > 0.5) {
            PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
            PIDC::confirm();
        }
        quadOmni.moveXY(
            pad.getStick(0) / 2.0,
            -pad.getStick(1) /2.0,
            PIDC::calculationResult
        );
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
}

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

    if(!pad.getButton2(2)) {
        PIDC::resetPlaneOffset();
    }

    if((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
        PIDC::PID::setSetPoint(0.0);
        PIDC::resetAxisOffset();
    }

    if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
        moment = pad.getStick(2) / 4.0;
        PIDC::confirm();
    }
    if(fabs(pad.getStick(2)) < 0.5) {
        PIDC::confirm();
        moment = PIDC::calculationResult;
    }

    if(receiveSuccessed) {
        quadOmni.moveCircular(
            pad.getNorm(0) / 2.0,
            pad.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
            moment
        );
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
    beforestick = pad.getStick(2);
}

void Bot::controllMech()
{
    if(receiveSuccessed) {
//        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)) armMotor[SWORD].setSpeed(0.6);
        if(!pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.6);
        if(pad.getButton1(5) && pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.0);

//
//        if(!pad.getButton1(2)) {
//            motor.destroy(DESTROY_MAX_SPEED);
//        } else {
//            motor.destroy(0);
//        }
//
       if(!pad.getButton2(1)) slider.release();
       if(!pad.getButton2(3)) powerSwitch = 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)
     ) {
        PIDC::calibration(HMC6352_ENTER_CALIB);
        quadOmni.moveXY(0, 0, 0.4);
        wait(5.0);
        quadOmni.moveXY(0, 0, 0);
        PIDC::calibration(HMC6352_EXIT_CALIB);
    }
}