2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
Diff: Spiral.cpp
- Revision:
- 23:ba636c80ed1b
- Parent:
- 22:d7c3bd1ce8f8
- Parent:
- 20:d052a0679309
diff -r d7c3bd1ce8f8 -r ba636c80ed1b Spiral.cpp --- a/Spiral.cpp Fri Nov 17 18:04:41 2017 +0900 +++ b/Spiral.cpp Fri Nov 17 18:08:21 2017 +0900 @@ -2,15 +2,26 @@ Spiral::Spiral(ikarashiMDC *spiralMotor_) { - beltSpeed = 0; + beltSpeed = 0.0; spiralSpeed = 0; spiralMotor = spiralMotor_; spiralLimit = new InterruptIn(PB_12); spiralLimit->mode(PullUp); spiralLimit->rise(this,&Spiral::stopRotation); + beltPID = new PID(60,1e-47,0.00005,0.01); rotating = false; firing = false; spiralLimit->enable_irq(); + + beltEncoder = new QEI(PA_11,PA_7,NC,720); + + beltPID->setOutputLimits(-1.0,1.0); + beltPID->setInputLimits(-10000,10000); + beltPID->setBias(0); + beltPID->setMode(AUTO_MODE); + beltPID->setSetPoint(0); + beltPos = 0; + } int Spiral::rotate() @@ -29,16 +40,20 @@ if (!rotating)return; spiralSpeed=0; spiralMotor->setSpeed(0); - rotating = false; beltTime.attach(callback(this,&Spiral::beltStart),0.1); } void Spiral::beltStart() { - beltSpeed=-1; - beltTime.attach(callback(this,&Spiral::beltStop),0.1); + beltEncoder->reset(); + beltPos =200; + beltPID->setSetPoint(beltPos); + rotating = false; } -void Spiral::beltStop(){ - beltSpeed=0; - firing = false; +void Spiral::update() +{ + beltPID->setProcessValue(beltEncoder->getPulses()/720.0*5*30); + beltPID->setSetPoint(beltPos); + beltSpeed = -beltPID->compute(); + if((beltEncoder->getPulses()/720.0*5*30 >beltPos-10) && rotating == false ) firing = false; }