2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
Diff: hayatoBoomerang.cpp
- Revision:
- 2:aca690545ec9
- Parent:
- 1:35e874e10602
- Child:
- 4:c2b09fa31492
diff -r 35e874e10602 -r aca690545ec9 hayatoBoomerang.cpp --- a/hayatoBoomerang.cpp Thu Aug 24 18:16:59 2017 +0900 +++ b/hayatoBoomerang.cpp Thu Aug 24 20:47:56 2017 +0900 @@ -1,39 +1,44 @@ #include "hayatoBoomerang.h" +#include "pinConfig.h" boomerang::boomerang(Serial *serial_) { - speedEncoder = new InteruputIn(SPEED_ENCODER); - armEncoder= new InteruputIn(ARM_ENCODER); + speedEncoder = new InterruptIn(SPEED_ENCODER); + armEncoder= new QEI(NC,NC,NC,720); - armLimit = new InteruptIn(ARM_LIMIT); - spiralLimit = new InteruptIn(SPIRAL_LIMIT); + 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); + speedPID->setInputLimits(0.0,6000.0); + speedPID->setOutputLimits(0,1.0); - armPID.setInputLimits(0,90) - speedPID.setOutputLimits(-1.0,1.0); + armPID->setInputLimits(0,90); + speedPID->setOutputLimits(-1.0,1.0); + + speedPID->setBias(0); + speedPID->setMode(AUTO_MODE); + speedPID->setSetPoint(0); - speedPID.setBias(0); - speedPID.setMode(AUTO); - speedPID.setSetPoint(0); + armPID->setBias(0); + armPID->setSetPoint(0); + armPID->setMode(AUTO_MODE); - armPID.setBias(0); - armPID.setSetPoint(0); - armPID.setMode(AUTO); + 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_); - armMotor = new IkarashiMDC(&rscs,0,0,serial_); - shootingMotor = new IkarashiMDC(&rscs,0,1,serial_); + spiral = new Spiral(spiralMotor); motorRPM = 0; - armEncoder = new QEI(ARM_ENCODER_A,ARM_ENCODER_B,NC,720,X4_ENCODING); - speedEncoder.rise(&shootingEncoderCount); - speedEncoder.enable_irq(); - armLimit.rise(&endCalibration); - armLimit.disable_irq(); + speedEncoder->rise(this,&boomerang::shootingEncoderCount); + speedEncoder->enable_irq(); + armLimit->rise(this,&boomerang::endCalibration); + armLimit->disable_irq(); } @@ -46,46 +51,47 @@ void boomerang::setTargetMotorSpeed(const int& targetRPM) { - speedPID.setSetPoint(targetRPM); + speedPID->setSetPoint(targetRPM); } void boomerang::setTargetArmAngle(const int& targetArmAngle) { - armPID.setSetPoint(targetArmAngle); + armPID->setSetPoint(targetArmAngle); } -void booomerang::update() +void boomerang::update() { - armMotor.setSpeed(armPID.getRealOutput()); - shootingMotor.setSpeed(speedPID.getRealOutput()); + armMotor->setSpeed(armPID->compute()); + shootingMotor->setSpeed(speedPID->compute()); } -void booomerang::fire() +bool boomerang::fire() { - beltMotor.setSpeed(0.4); - beltTime.attach(beltStop,0.5); - spiral.reload(); + beltMotor->setSpeed(0.4); + beltTime.attach(this,&boomerang::beltStop,0.5); + spiral->rotate(); + return true; } void boomerang::beltStop() { - beltMotor.setSpeed(0); + beltMotor->setSpeed(0); } int boomerang::calibrateArm() { - if(armLimit == true) + if(armLimit->read() == true) return 1; - armLimit.enable_irq(); - armMotor.setSpeed(0.2); + armLimit->enable_irq(); + armMotor->setSpeed(0.2); return 0; } void boomerang::endCalibration() { - armEncoder.reset() - armLimit.disable_irq(); - armPID.setSetPoint(10); + armEncoder->reset(); + armLimit->disable_irq(); + armPID->setSetPoint(10); }