![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
hayatoBoomerang.cpp@2:aca690545ec9, 2017-08-24 (annotated)
- Committer:
- WAT34
- Date:
- Thu Aug 24 20:47:56 2017 +0900
- Revision:
- 2:aca690545ec9
- Parent:
- 1:35e874e10602
- Child:
- 4:c2b09fa31492
working commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 1:35e874e10602 | 1 | #include "hayatoBoomerang.h" |
WAT34 | 2:aca690545ec9 | 2 | #include "pinConfig.h" |
WAT34 | 1:35e874e10602 | 3 | |
WAT34 | 1:35e874e10602 | 4 | boomerang::boomerang(Serial *serial_) |
WAT34 | 1:35e874e10602 | 5 | { |
WAT34 | 2:aca690545ec9 | 6 | speedEncoder = new InterruptIn(SPEED_ENCODER); |
WAT34 | 2:aca690545ec9 | 7 | armEncoder= new QEI(NC,NC,NC,720); |
WAT34 | 1:35e874e10602 | 8 | |
WAT34 | 2:aca690545ec9 | 9 | armLimit = new InterruptIn(ARM_LIMIT); |
WAT34 | 2:aca690545ec9 | 10 | |
WAT34 | 2:aca690545ec9 | 11 | rscs = new DigitalOut(NC); |
WAT34 | 1:35e874e10602 | 12 | |
WAT34 | 1:35e874e10602 | 13 | speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE); |
WAT34 | 1:35e874e10602 | 14 | armPID = new PID(ARM_P,ARM_I,ARM_D,RATE); |
WAT34 | 2:aca690545ec9 | 15 | speedPID->setInputLimits(0.0,6000.0); |
WAT34 | 2:aca690545ec9 | 16 | speedPID->setOutputLimits(0,1.0); |
WAT34 | 1:35e874e10602 | 17 | |
WAT34 | 2:aca690545ec9 | 18 | armPID->setInputLimits(0,90); |
WAT34 | 2:aca690545ec9 | 19 | speedPID->setOutputLimits(-1.0,1.0); |
WAT34 | 2:aca690545ec9 | 20 | |
WAT34 | 2:aca690545ec9 | 21 | speedPID->setBias(0); |
WAT34 | 2:aca690545ec9 | 22 | speedPID->setMode(AUTO_MODE); |
WAT34 | 2:aca690545ec9 | 23 | speedPID->setSetPoint(0); |
WAT34 | 1:35e874e10602 | 24 | |
WAT34 | 2:aca690545ec9 | 25 | armPID->setBias(0); |
WAT34 | 2:aca690545ec9 | 26 | armPID->setSetPoint(0); |
WAT34 | 2:aca690545ec9 | 27 | armPID->setMode(AUTO_MODE); |
WAT34 | 1:35e874e10602 | 28 | |
WAT34 | 2:aca690545ec9 | 29 | armMotor = new ikarashiMDC(rscs,0,0,SM,serial_); |
WAT34 | 2:aca690545ec9 | 30 | shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_); |
WAT34 | 2:aca690545ec9 | 31 | spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_); |
WAT34 | 2:aca690545ec9 | 32 | beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_); |
WAT34 | 1:35e874e10602 | 33 | |
WAT34 | 2:aca690545ec9 | 34 | spiral = new Spiral(spiralMotor); |
WAT34 | 1:35e874e10602 | 35 | |
WAT34 | 1:35e874e10602 | 36 | motorRPM = 0; |
WAT34 | 1:35e874e10602 | 37 | |
WAT34 | 2:aca690545ec9 | 38 | speedEncoder->rise(this,&boomerang::shootingEncoderCount); |
WAT34 | 2:aca690545ec9 | 39 | speedEncoder->enable_irq(); |
WAT34 | 2:aca690545ec9 | 40 | armLimit->rise(this,&boomerang::endCalibration); |
WAT34 | 2:aca690545ec9 | 41 | armLimit->disable_irq(); |
WAT34 | 1:35e874e10602 | 42 | |
WAT34 | 1:35e874e10602 | 43 | } |
WAT34 | 1:35e874e10602 | 44 | |
WAT34 | 1:35e874e10602 | 45 | void boomerang::shootingEncoderCount() |
WAT34 | 1:35e874e10602 | 46 | { |
WAT34 | 1:35e874e10602 | 47 | motorRPM = (1.0/rpmCalc.read())*60; |
WAT34 | 1:35e874e10602 | 48 | rpmCalc.reset(); |
WAT34 | 1:35e874e10602 | 49 | rpmCalc.start(); |
WAT34 | 1:35e874e10602 | 50 | } |
WAT34 | 1:35e874e10602 | 51 | |
WAT34 | 1:35e874e10602 | 52 | void boomerang::setTargetMotorSpeed(const int& targetRPM) |
WAT34 | 1:35e874e10602 | 53 | { |
WAT34 | 2:aca690545ec9 | 54 | speedPID->setSetPoint(targetRPM); |
WAT34 | 1:35e874e10602 | 55 | } |
WAT34 | 1:35e874e10602 | 56 | |
WAT34 | 1:35e874e10602 | 57 | void boomerang::setTargetArmAngle(const int& targetArmAngle) |
WAT34 | 1:35e874e10602 | 58 | { |
WAT34 | 2:aca690545ec9 | 59 | armPID->setSetPoint(targetArmAngle); |
WAT34 | 1:35e874e10602 | 60 | } |
WAT34 | 1:35e874e10602 | 61 | |
WAT34 | 2:aca690545ec9 | 62 | void boomerang::update() |
WAT34 | 1:35e874e10602 | 63 | { |
WAT34 | 2:aca690545ec9 | 64 | armMotor->setSpeed(armPID->compute()); |
WAT34 | 2:aca690545ec9 | 65 | shootingMotor->setSpeed(speedPID->compute()); |
WAT34 | 1:35e874e10602 | 66 | } |
WAT34 | 1:35e874e10602 | 67 | |
WAT34 | 2:aca690545ec9 | 68 | bool boomerang::fire() |
WAT34 | 1:35e874e10602 | 69 | { |
WAT34 | 2:aca690545ec9 | 70 | beltMotor->setSpeed(0.4); |
WAT34 | 2:aca690545ec9 | 71 | beltTime.attach(this,&boomerang::beltStop,0.5); |
WAT34 | 2:aca690545ec9 | 72 | spiral->rotate(); |
WAT34 | 1:35e874e10602 | 73 | |
WAT34 | 2:aca690545ec9 | 74 | return true; |
WAT34 | 1:35e874e10602 | 75 | } |
WAT34 | 1:35e874e10602 | 76 | |
WAT34 | 1:35e874e10602 | 77 | void boomerang::beltStop() |
WAT34 | 1:35e874e10602 | 78 | { |
WAT34 | 2:aca690545ec9 | 79 | beltMotor->setSpeed(0); |
WAT34 | 1:35e874e10602 | 80 | } |
WAT34 | 1:35e874e10602 | 81 | |
WAT34 | 1:35e874e10602 | 82 | int boomerang::calibrateArm() |
WAT34 | 1:35e874e10602 | 83 | { |
WAT34 | 2:aca690545ec9 | 84 | if(armLimit->read() == true) |
WAT34 | 1:35e874e10602 | 85 | return 1; |
WAT34 | 1:35e874e10602 | 86 | |
WAT34 | 2:aca690545ec9 | 87 | armLimit->enable_irq(); |
WAT34 | 2:aca690545ec9 | 88 | armMotor->setSpeed(0.2); |
WAT34 | 1:35e874e10602 | 89 | return 0; |
WAT34 | 1:35e874e10602 | 90 | } |
WAT34 | 1:35e874e10602 | 91 | |
WAT34 | 1:35e874e10602 | 92 | void boomerang::endCalibration() |
WAT34 | 1:35e874e10602 | 93 | { |
WAT34 | 2:aca690545ec9 | 94 | armEncoder->reset(); |
WAT34 | 2:aca690545ec9 | 95 | armLimit->disable_irq(); |
WAT34 | 2:aca690545ec9 | 96 | armPID->setSetPoint(10); |
WAT34 | 1:35e874e10602 | 97 | } |