2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
Diff: hayatoBoomerang.cpp
- Revision:
- 1:35e874e10602
- Child:
- 2:aca690545ec9
diff -r ef0a124a6c64 -r 35e874e10602 hayatoBoomerang.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hayatoBoomerang.cpp Thu Aug 24 18:16:59 2017 +0900 @@ -0,0 +1,91 @@ +#include "hayatoBoomerang.h" + +boomerang::boomerang(Serial *serial_) +{ + speedEncoder = new InteruputIn(SPEED_ENCODER); + armEncoder= new InteruputIn(ARM_ENCODER); + + armLimit = new InteruptIn(ARM_LIMIT); + spiralLimit = new InteruptIn(SPIRAL_LIMIT); + + 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); + speedPID.setSetPoint(0); + + armPID.setBias(0); + armPID.setSetPoint(0); + armPID.setMode(AUTO); + + armMotor = new IkarashiMDC(&rscs,0,0,serial_); + shootingMotor = new IkarashiMDC(&rscs,0,1,serial_); + + 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(); + +} + +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 booomerang::update() +{ + armMotor.setSpeed(armPID.getRealOutput()); + shootingMotor.setSpeed(speedPID.getRealOutput()); +} + +void booomerang::fire() +{ + beltMotor.setSpeed(0.4); + beltTime.attach(beltStop,0.5); + spiral.reload(); + +} + +void boomerang::beltStop() +{ + beltMotor.setSpeed(0); +} + +int boomerang::calibrateArm() +{ + if(armLimit == true) + return 1; + + armLimit.enable_irq(); + armMotor.setSpeed(0.2); + return 0; +} + +void boomerang::endCalibration() +{ + armEncoder.reset() + armLimit.disable_irq(); + armPID.setSetPoint(10); +}