NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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?

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