NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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?

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