2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
hayatoBoomerang.cpp
- Committer:
- WAT34
- Date:
- 2017-09-01
- Revision:
- 6:0bee4b2bb400
- Parent:
- 4:c2b09fa31492
- Child:
- 8:c47cf4e0230c
- Child:
- 11:68defd561031
File content as of revision 6:0bee4b2bb400:
#include "hayatoBoomerang.h" #include "pinConfig.h" boomerang::boomerang(Serial *serial_) { speedEncoder = new InterruptIn(SPEED_ENCODER); armEncoder= new QEI(D10,D11,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,9000.0); speedPID->setOutputLimits(0,1.0); armPID->setInputLimits(5,80); 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); 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)/127.0,(x-127)/127.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); }