File content as of revision 52:320f910ca6ca:

#include "bot.h"

Bot::Bot() :
    pad1(XBee1TX, XBee1RX, ADDR1),
    pad2(XBee2TX, XBee2RX, ADDR2),
    RS485(MDTX, MDRX, 115200),
    RS485Controller(PWM1),
    powerSwitch(MDstop),
    quadOmni(&RS485Controller, &RS485),
    tentacle(&RS485Controller, &RS485),
    nishijoSword(&RS485Controller, &RS485),
    nishijo(&RS485Controller, &RS485),
    plane(),
    axis(),
    receiveSuccessed1(0),
    receiveSuccessed2(0),
    frontDegree(0),
    debugSerial(USBTX, USBRX, 115200)
{
    debugSerial.printf("OK\r\n");

    quadOmni.moveXY(0, 0, 0);
    tentacle.stop();
    nishijoSword.stop();
    nishijo.stop();


    powerSwitch = true;
}

void Bot::confirmPad1()
{
    receiveSuccessed1 = pad1.receiveState();
    if(!pad1.getButton2(0) && !pad1.getButton2(1)) {
        powerSwitch = 0;
    }
    if(!pad1.getButton2(2) && !pad1.getButton2(3)) {
        powerSwitch = 1;
    }
    if(!receiveSuccessed1) {
        quadOmni.moveXY(0, 0, 0);
    }
}

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

void Bot::controllDrive()
{
    if(receiveSuccessed1) {
        float moment = 0;
        //debugSerial.printf("%d\n\r", plane.getRawDegree());
        /*if(pad1.getNorm(0) > 0.5) {
            plane.setPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
            plane.confirm();
        }*/
        plane.confirm();
        if(pad1.getStick(0)) {
          moment = -pad1.getStick(0) / 2.0;
          plane.resetOffset();
        } else {
          moment = -plane.getCalculationResult();
        }
        debugSerial.printf("%f\r\n",moment);
        quadOmni.moveXY(
                pad1.getStick(2),
                -pad1.getStick(3),
                0,
                0,
                moment
                );
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
}

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

  if(!pad1.getButton2(5)) {
      axis.resetOffset();
      //debugSerial.printf("Force RESeT\n\r");
  }

  if((beforestick >= 0.2 && pad1.getStick(0) < 0.2) || (beforestick <= -0.2 && pad1.getStick(0) > -0.2)) {
      plane.setPoint(0.0);
      plane.resetOffset();
  }
  /*if(!pad1.getButton2(1)&&pad1.getButton2(3)) {
        plane.setPoint((M_PI / 4)*(180.0 / M_PI));
      } else if(pad1.getButton2(1)&&!pad1.getButton2(3)){
        plane.setPoint(-(M_PI / 4)*(180.0 / M_PI));
      } else {
        plane.setPoint(0.0);
  }*/
  if(pad1.getStick(0) > 0.2 || pad1.getStick(0) < -0.2) {
      moment = pad1.getStick(0) / 2.0;
  } else  {
      moment = plane.getCalculationResult();

  }
  if(pad1.getButton1(0)) {
      norm = pad1.getNorm(1);
  } else {
      norm = pad1.getNorm(1) / 2.0;
  }
  if(receiveSuccessed1) {
      quadOmni.moveCircular(
              norm,
              pad1.getRadian(1) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
              0.0,
              0.0,
              -moment
              );
  } else {
      quadOmni.moveXY(0, 0, 0);
  }
  beforestick = pad1.getStick(0);
}

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::controllDrive4()
{
    if(receiveSuccessed1) {
        quadOmni.moveXY(
                pad1.getStick(2),
                pad1.getStick(3),
                0.0,
                0.0,
                -pad1.getStick(0)/2.0
                );
    } else {
        quadOmni.moveXY(0, 0, 0);
    }
}
void Bot::controllMech()
{
  if(receiveSuccessed2) {

      //if(!pad2.getButton1(2)&&!pad2.getButton1(4)) powerSwitch=0;
    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(3)) nishijo.move(-SWORD_SPEED);
    if (!pad2.getButton2(2)) nishijo.move(SWORD_SPEED);
    if(pad2.getButton2(2) && pad2.getButton2(3))  nishijo.move(0);
  } else {
    tentacle.stop();
    nishijoSword.stop();
    nishijo.stop();
  }
}

void Bot::calibrate()
{
}

void Bot::checkConnection()
{
  if(receiveSuccessed1 == 1) debugSerial.printf("ON ");
  else  debugSerial.printf("%d ",receiveSuccessed1);

  if(receiveSuccessed2 == 1) debugSerial.printf("ON\r\n");
  else  debugSerial.printf("%d\r\n",receiveSuccessed2);
}

void Bot::checkDegree()
{
    debugSerial.printf("%f %f\r\n",plane.getRawDegree(), plane.getCalculationResult());
}

void Bot::checkPacket()
{
    char buf=0;
    while(1) {
      buf =  pad1.getc();
      debugSerial.printf("%d %c\r\n",buf,buf);
    }
}

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