NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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?

UserRevisionLine numberNew 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 }