File content as of revision 16:5f205692b14d:

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

boomerang::boomerang(Serial *serial_)
{
  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,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;

  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();
  armLimit->fall(this,&boomerang::endCalibration);
  armLimit->disable_irq();

  firing = false;

  boomerang::update();
}

void boomerang::move(const uint8_t& x,const uint8_t& y,const uint8_t& 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)-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::setTargetArmAngle(const int& targetArmAngle)
{
    armPID->setSetPoint(targetArmAngle);
}

void boomerang::update()
{
  armAngle = armEncoder->getPulses()/800.0*85.51;
  armPID->setProcessValue(armAngle);
  speedPID->setProcessValue(motorRPM);
  if(!calibrating)
    armMotor->setSpeed(armPID->compute());
  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();

}

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

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

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