2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
Spiral.cpp@23:ba636c80ed1b, 2017-11-17 (annotated)
- Committer:
- WAT34
- Date:
- Fri Nov 17 18:08:21 2017 +0900
- Revision:
- 23:ba636c80ed1b
- Parent:
- 22:d7c3bd1ce8f8
- Parent:
- 20:d052a0679309
merged
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 3:01947ec86f09 | 1 | #include "Spiral.h" |
WAT34 | 3:01947ec86f09 | 2 | |
WAT34 | 17:311aed3cad15 | 3 | Spiral::Spiral(ikarashiMDC *spiralMotor_) |
WAT34 | 3:01947ec86f09 | 4 | { |
WAT34 | 20:d052a0679309 | 5 | beltSpeed = 0.0; |
WAT34 | 17:311aed3cad15 | 6 | spiralSpeed = 0; |
WAT34 | 3:01947ec86f09 | 7 | spiralMotor = spiralMotor_; |
eil4nyqn | 8:c47cf4e0230c | 8 | spiralLimit = new InterruptIn(PB_12); |
WAT34 | 6:0bee4b2bb400 | 9 | spiralLimit->mode(PullUp); |
eil4nyqn | 8:c47cf4e0230c | 10 | spiralLimit->rise(this,&Spiral::stopRotation); |
WAT34 | 20:d052a0679309 | 11 | beltPID = new PID(60,1e-47,0.00005,0.01); |
WAT34 | 3:01947ec86f09 | 12 | rotating = false; |
WAT34 | 17:311aed3cad15 | 13 | firing = false; |
WAT34 | 17:311aed3cad15 | 14 | spiralLimit->enable_irq(); |
WAT34 | 20:d052a0679309 | 15 | |
WAT34 | 20:d052a0679309 | 16 | beltEncoder = new QEI(PA_11,PA_7,NC,720); |
WAT34 | 20:d052a0679309 | 17 | |
WAT34 | 20:d052a0679309 | 18 | beltPID->setOutputLimits(-1.0,1.0); |
WAT34 | 20:d052a0679309 | 19 | beltPID->setInputLimits(-10000,10000); |
WAT34 | 20:d052a0679309 | 20 | beltPID->setBias(0); |
WAT34 | 20:d052a0679309 | 21 | beltPID->setMode(AUTO_MODE); |
WAT34 | 20:d052a0679309 | 22 | beltPID->setSetPoint(0); |
WAT34 | 20:d052a0679309 | 23 | beltPos = 0; |
WAT34 | 20:d052a0679309 | 24 | |
WAT34 | 3:01947ec86f09 | 25 | } |
WAT34 | 3:01947ec86f09 | 26 | |
WAT34 | 3:01947ec86f09 | 27 | int Spiral::rotate() |
WAT34 | 3:01947ec86f09 | 28 | { |
WAT34 | 3:01947ec86f09 | 29 | if(rotating) |
WAT34 | 3:01947ec86f09 | 30 | return 1; |
WAT34 | 22:d7c3bd1ce8f8 | 31 | spiralSpeed=-.5; |
WAT34 | 22:d7c3bd1ce8f8 | 32 | spiralMotor->setSpeed(-.5); |
WAT34 | 3:01947ec86f09 | 33 | rotating = true; |
WAT34 | 17:311aed3cad15 | 34 | firing = true; |
WAT34 | 3:01947ec86f09 | 35 | return false; |
WAT34 | 3:01947ec86f09 | 36 | } |
WAT34 | 3:01947ec86f09 | 37 | |
WAT34 | 3:01947ec86f09 | 38 | void Spiral::stopRotation() |
WAT34 | 3:01947ec86f09 | 39 | { |
WAT34 | 17:311aed3cad15 | 40 | if (!rotating)return; |
WAT34 | 17:311aed3cad15 | 41 | spiralSpeed=0; |
WAT34 | 17:311aed3cad15 | 42 | spiralMotor->setSpeed(0); |
WAT34 | 18:4b629221c215 | 43 | beltTime.attach(callback(this,&Spiral::beltStart),0.1); |
WAT34 | 3:01947ec86f09 | 44 | } |
WAT34 | 17:311aed3cad15 | 45 | |
WAT34 | 18:4b629221c215 | 46 | void Spiral::beltStart() |
WAT34 | 18:4b629221c215 | 47 | { |
WAT34 | 20:d052a0679309 | 48 | beltEncoder->reset(); |
WAT34 | 20:d052a0679309 | 49 | beltPos =200; |
WAT34 | 20:d052a0679309 | 50 | beltPID->setSetPoint(beltPos); |
WAT34 | 20:d052a0679309 | 51 | rotating = false; |
WAT34 | 18:4b629221c215 | 52 | } |
WAT34 | 20:d052a0679309 | 53 | void Spiral::update() |
WAT34 | 20:d052a0679309 | 54 | { |
WAT34 | 20:d052a0679309 | 55 | beltPID->setProcessValue(beltEncoder->getPulses()/720.0*5*30); |
WAT34 | 20:d052a0679309 | 56 | beltPID->setSetPoint(beltPos); |
WAT34 | 20:d052a0679309 | 57 | beltSpeed = -beltPID->compute(); |
WAT34 | 20:d052a0679309 | 58 | if((beltEncoder->getPulses()/720.0*5*30 >beltPos-10) && rotating == false ) firing = false; |
WAT34 | 17:311aed3cad15 | 59 | } |