File content as of revision 25:d199d621ecca:

#include "bot.h"

Bot::Bot() :
    PIDC(),
    pad1(XBee1TX, XBee1RX, ADDR1),
    pad2(XBee2TX, XBee2RX, ADDR2),
    RS485(MDTX, MDRX, 38400),
    RS485Controller(PWM2),
    powerSwitch(MDstop),
    quadOmni(&RS485Controller, &RS485),
    tentacle(&RS485Controller, &RS485),
    nishijo(&RS485Controller, &RS485),
    nishijoSword(&RS485Controller, &RS485),
    receiveSuccessed1(0),
    receiveSuccessed2(0),
    led({DebugLED1, DebugLED2, DebugLED3, DebugLED4,
        DebugLED5, DebugLED6, DebugLED7, DebugLED8}),
    test(PWM1),
    debugSerial(USBTX, USBRX, 230400)
{

    test = 0;
    quadOmni.moveXY(0, 0, 0);

    powerSwitch = 1;

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

void Bot::confirmAll()
{
    receiveSuccessed1 = pad1.receiveState();
    receiveSuccessed2 = pad2.receiveState();
    if(!receiveSuccessed1) {
        quadOmni.moveXY(0, 0, 0);
        tentacle.stop();
    }
}

void Bot::controllDrive()
{
    test = !test;
    if(receiveSuccessed1) {
        if(pad1.getNorm(0) > 0.5) {
            PIDC::PID::setSetPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
            PIDC::confirm();
        }
        //PIDC::confirm();
        PIDC::renewAngle();
        //test = !test;
        quadOmni.moveXY(
            pad1.getStick(0) * 0.8,
            -pad1.getStick(1) * 0.8,
            pad1.getStick(2) * 0.4
             //PIDC::calculationResult
        );
        /*quadOmni.moveCircular(
          0.2,
          0,
          0
        );*/
    } else {
        quadOmni.moveXY(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(receiveSuccessed1) {
        quadOmni.moveCircular(
            pad1.getNorm(0) / 2.0,
            pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
            moment
        );
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
    beforestick = pad1.getStick(2);
}

void Bot::controllMech()
{
    //nishijoSword.move(1.0);
    if(receiveSuccessed2) {
        /*if(!pad1.getButton2(2)) tentacle.rightMove(TENTACLE_SPEED);
        if(!pad1.getButton2(3)) tentacle.rightMove(-TENTACLE_SPEED);
        if(pad1.getButton2(2) && pad1.getButton2(3))  tentacle.rightMove(0);

        if(!pad1.getButton2(0)) tentacle.leftMove(TENTACLE_SPEED);
        if(!pad1.getButton2(1)) tentacle.leftMove(-TENTACLE_SPEED);
        if(pad1.getButton2(0) && pad1.getButton2(1))  tentacle.leftMove(0);*/

        if(pad2.getStick(1) > 0){
            tentacle.leftMove(pad2.getStick(1));
        } else {
            tentacle.leftMove(-TENTACLE_SPEED);
        }

        if(pad2.getStick(3) > 0){
            tentacle.leftMove(pad2.getStick(3));
        } else {
            tentacle.leftMove(-TENTACLE_SPEED);
        }

        if (!pad1.getButton2(3)) nishijoSword.move(WIND_UP_SPEED);
        if (!pad1.getButton2(5)) nishijoSword.move(-WIND_UP_SPEED);
        if(pad2.getButton2(3) && pad2.getButton2(5))  nishijoSword.move(0);

        //test = !pad1.getButton1(5);

        //if(!pad1.getButton1(6)) powerSwitch = 0;
        //if(!pad1.getButton1(5)&&!pad1.getButton1(4)) powerSwitch = 1;

        //else powerSwitch = 1;
    } else {
      tentacle.stop();
      nishijoSword.stop();
      //test =0;
      //powerSwitch = 0;
    }
    /*if(receiveSuccessed2){

    }*/
}


void Bot::calibrate()
{

}

void Bot::debugPrint()
{
  if(receiveSuccessed1) {
    debugSerial.printf("%f %f %f %f \r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3));
  } else {
    debugSerial.printf("\r\n");
  }
  if(receiveSuccessed2) {
    debugSerial.printf("%f %f %f %f \r\n",pad2.getStick(0),pad2.getStick(1),pad2.getStick(2),pad2.getStick(3));

  } else {
    debugSerial.printf("\r\n");
  }
}