2017ロボコンはやとブーメランプログラム

Dependencies:   PID QEI ikarashiMDC recieveController omni

hayatoBoomerang.cpp

Committer:
WAT34
Date:
2017-09-01
Revision:
11:68defd561031
Parent:
6:0bee4b2bb400
Child:
12:9d21189a8eb0

File content as of revision 11:68defd561031:

#include "hayatoBoomerang.h"
#include "pinConfig.h"

boomerang::boomerang(Serial *serial_)
{
  speedEncoder = new InterruptIn(SPEED_ENCODER);

  //armLimit->mode(PullUp);

  rscs = new DigitalOut(NC);

  speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE);
  speedPID->setInputLimits(0.0,9000.0);
  speedPID->setOutputLimits(0,1.0);


  speedPID->setBias(0);
  speedPID->setMode(AUTO_MODE);
  speedPID->setSetPoint(0);


  armValve[0] = new DigitalOut(D13);
  armValve[1] = new DigitalOut(D13);
  shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
  spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
  beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);

  omni = new Omni(4);
  omniMotor[0] = new ikarashiMDC(rscs,1,0,SM,serial_);
  omniMotor[1] = new ikarashiMDC(rscs,1,1,SM,serial_);
  omniMotor[2] = new ikarashiMDC(rscs,1,2,SM,serial_);
  omniMotor[3] = new ikarashiMDC(rscs,1,3,SM,serial_);
  omniMotor[0]->braking=true;
  omniMotor[1]->braking=true;
  omniMotor[2]->braking=true;
  omniMotor[3]->braking=true;

  armValve[0]->write(false);
  armValve[1]->write(false);
  shootingMotor->setSpeed(0);
  spiralMotor->setSpeed(0);
  beltMotor->setSpeed(0);

  spiral = new Spiral(spiralMotor);

  motorRPM = 0;

  omni->setWheelRadian(radians(45),radians(45+90),radians(45+180),radians(45+270));

  speedEncoder->rise(this,&boomerang::shootingEncoderCount);
  speedEncoder->enable_irq();
  firing = false;

  boomerang::update();
}

void boomerang::move(const uint8_t& x,const uint8_t& y,const uint8_t& moment)
{
    omni->computeXY((y-127)/127.0,(x-127)/127.0,moment);
    for (size_t i = 0; i < 4; i++) {
      omniMotor[i]->setSpeed((omni->getOutput(i)-127)/127.0);
    }
}

void boomerang::shootingEncoderCount()
{
  motorRPM = (1.0/rpmCalc.read())*60;
  rpmCalc.reset();
  rpmCalc.start();
}

void boomerang::setTargetMotorSpeed(const int& targetRPM)
{
  speedPID->setSetPoint(targetRPM);
}


void boomerang::update()
{
  speedPID->setProcessValue(motorRPM);
  shootingMotor->setSpeed(speedPID->compute());
  beltMotor->setSpeed(beltSpeed);
}

bool boomerang::fire()
{
  if(firing)
    return false;
  firing = true;
  beltSpeed=0.4;
  beltTime.attach(callback(this,&boomerang::beltStop),1);
  spiral->rotate();
  boomerang::update();
  return true;
}

void boomerang::beltStop()
{
  beltSpeed = 0;
  firing = false;
  boomerang::update();

}

void boomerang::sword()
{
  armValve[0]->write(!armValve[0]->read());
  armValve[1]->write(!armValve[0]->read());
}