![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
hayatoBoomerang.cpp
- Committer:
- WAT34
- Date:
- 2017-08-29
- Revision:
- 4:c2b09fa31492
- Parent:
- 2:aca690545ec9
- Child:
- 6:0bee4b2bb400
File content as of revision 4:c2b09fa31492:
#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(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,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_); armMotor->setSpeed(0); shootingMotor->setSpeed(0); spiralMotor->setSpeed(0); beltMotor->setSpeed(0); spiral = new Spiral(spiralMotor); motorRPM = 0; speedEncoder->rise(this,&boomerang::shootingEncoderCount); speedEncoder->enable_irq(); armLimit->fall(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() { 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),0.5); 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); }