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