File content as of revision 18:4b629221c215:

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

boomerang::boomerang(Serial *serial_,Serial *serial2_)
{
  spiralSerial = serial2_;
  speedEncoder = new InterruptIn(SPEED_ENCODER);
  armEncoder= new QEI(PB_15,PB_4,NC,624);

  armLimit = new InterruptIn(NC);
  //armLimit->mode(PullUp);

  rscs = new DigitalOut(NC);

  speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE);
  armPID = new PID(ARM_P,ARM_I,ARM_D,RATE);
  speedPID->setInputLimits(0.0,15000.0);
  speedPID->setOutputLimits(0,1.0);

  armPID->setInputLimits(0,81);
  armPID->setOutputLimits(-1.0,1.0);

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

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

  armMotor = new ikarashiMDC(rscs,0,0,SM,serial_);
  shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
  spiralMotor = new ikarashiMDC(rscs,0,0,SM,spiralSerial);
  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;

  shootingMotor->braking=false;

  spiralMotor->braking=true;

  armMotor->setSpeed(0);
  shootingMotor->setSpeed(0);
  spiralMotor->setSpeed(0);
  beltMotor->setSpeed(0);
  omniMotor[0]->setSpeed(0);
  omniMotor[1]->setSpeed(0);
  omniMotor[2]->setSpeed(0);
  omniMotor[3]->setSpeed(0);

  spiral = new Spiral(spiralMotor);

  motorRPM = 0;

  omni->setWheelRadian(radians(51.675),radians(76.65+51.675),radians(76.65+51.675+103.35),radians(360-51.675));

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


}

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

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

void boomerang::setTargetMotorSpeed(const int& targetRPM)
{
  if(targetRPM == 0){
    speedPID->reset();
    speedPID->setSetPoint(0);
    return;
  }
  speedPID->setSetPoint(targetRPM);
}

void boomerang::setTargetArmAngle(const int& targetArmAngle)
{
    armPID->setSetPoint(targetArmAngle);
}

void boomerang::update()
{
  armAngle = armEncoder->getPulses()/800.0*85.51;
  armPID->setProcessValue(armAngle);
  speedPID->setProcessValue(motorRPM);
  armMotor->setSpeed(armPID->compute());
  shootingMotor->setSpeed(speedPID->compute());
  beltMotor->setSpeed(spiral->beltSpeed);
  spiralMotor->setSpeed(spiral->spiralSpeed);
}

bool boomerang::fire()
{
  if(spiral->firing)
    return false;
  spiral->rotate();
  return true;
}