File content as of revision 2:aca690545ec9:

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

boomerang::boomerang(Serial *serial_)
{
  speedEncoder = new InterruptIn(SPEED_ENCODER);
  armEncoder= new QEI(NC,NC,NC,720);

  armLimit = new InterruptIn(ARM_LIMIT);

  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,6000.0);
  speedPID->setOutputLimits(0,1.0);

  armPID->setInputLimits(0,90);
  speedPID->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,2,SM,serial_);
  beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);

  spiral = new Spiral(spiralMotor);

  motorRPM = 0;

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

}

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

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

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

void boomerang::update()
{
  armMotor->setSpeed(armPID->compute());
  shootingMotor->setSpeed(speedPID->compute());
}

bool boomerang::fire()
{
  beltMotor->setSpeed(0.4);
  beltTime.attach(this,&boomerang::beltStop,0.5);
  spiral->rotate();

  return true;
}

void boomerang::beltStop()
{
  beltMotor->setSpeed(0);
}

int boomerang::calibrateArm()
{
  if(armLimit->read() == true)
    return 1;

  armLimit->enable_irq();
  armMotor->setSpeed(0.2);
  return 0;
}

void boomerang::endCalibration()
{
  armEncoder->reset();
  armLimit->disable_irq();
  armPID->setSetPoint(10);
}