NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

2017ロボコンはやとブーメランプログラム

Dependencies:   PID QEI ikarashiMDC recieveController omni

hayatoBoomerang.cpp@4:c2b09fa31492, 2017-08-29 (annotated)

Committer:
WAT34
Date:
Tue Aug 29 17:00:05 2017 +0900
Revision:
4:c2b09fa31492
Parent:
2:aca690545ec9
Child:
6:0bee4b2bb400
debugs

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 4:c2b09fa31492 9 armLimit = new InterruptIn(NC);
WAT34 4:c2b09fa31492 10 //armLimit->mode(PullUp);
WAT34 2:aca690545ec9 11
WAT34 2:aca690545ec9 12 rscs = new DigitalOut(NC);
WAT34 1:35e874e10602 13
WAT34 1:35e874e10602 14 speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE);
WAT34 1:35e874e10602 15 armPID = new PID(ARM_P,ARM_I,ARM_D,RATE);
WAT34 2:aca690545ec9 16 speedPID->setInputLimits(0.0,6000.0);
WAT34 2:aca690545ec9 17 speedPID->setOutputLimits(0,1.0);
WAT34 1:35e874e10602 18
WAT34 2:aca690545ec9 19 armPID->setInputLimits(0,90);
WAT34 2:aca690545ec9 20 speedPID->setOutputLimits(-1.0,1.0);
WAT34 2:aca690545ec9 21
WAT34 2:aca690545ec9 22 speedPID->setBias(0);
WAT34 2:aca690545ec9 23 speedPID->setMode(AUTO_MODE);
WAT34 2:aca690545ec9 24 speedPID->setSetPoint(0);
WAT34 1:35e874e10602 25
WAT34 2:aca690545ec9 26 armPID->setBias(0);
WAT34 2:aca690545ec9 27 armPID->setSetPoint(0);
WAT34 2:aca690545ec9 28 armPID->setMode(AUTO_MODE);
WAT34 1:35e874e10602 29
WAT34 2:aca690545ec9 30 armMotor = new ikarashiMDC(rscs,0,0,SM,serial_);
WAT34 2:aca690545ec9 31 shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
WAT34 2:aca690545ec9 32 spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
WAT34 2:aca690545ec9 33 beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
WAT34 1:35e874e10602 34
WAT34 4:c2b09fa31492 35 armMotor->setSpeed(0);
WAT34 4:c2b09fa31492 36 shootingMotor->setSpeed(0);
WAT34 4:c2b09fa31492 37 spiralMotor->setSpeed(0);
WAT34 4:c2b09fa31492 38 beltMotor->setSpeed(0);
WAT34 4:c2b09fa31492 39
WAT34 2:aca690545ec9 40 spiral = new Spiral(spiralMotor);
WAT34 1:35e874e10602 41
WAT34 1:35e874e10602 42 motorRPM = 0;
WAT34 1:35e874e10602 43
WAT34 2:aca690545ec9 44 speedEncoder->rise(this,&boomerang::shootingEncoderCount);
WAT34 2:aca690545ec9 45 speedEncoder->enable_irq();
WAT34 4:c2b09fa31492 46 armLimit->fall(this,&boomerang::endCalibration);
WAT34 2:aca690545ec9 47 armLimit->disable_irq();
WAT34 1:35e874e10602 48
WAT34 1:35e874e10602 49 }
WAT34 1:35e874e10602 50
WAT34 1:35e874e10602 51 void boomerang::shootingEncoderCount()
WAT34 1:35e874e10602 52 {
WAT34 1:35e874e10602 53 motorRPM = (1.0/rpmCalc.read())*60;
WAT34 1:35e874e10602 54 rpmCalc.reset();
WAT34 1:35e874e10602 55 rpmCalc.start();
WAT34 1:35e874e10602 56 }
WAT34 1:35e874e10602 57
WAT34 1:35e874e10602 58 void boomerang::setTargetMotorSpeed(const int& targetRPM)
WAT34 1:35e874e10602 59 {
WAT34 2:aca690545ec9 60 speedPID->setSetPoint(targetRPM);
WAT34 1:35e874e10602 61 }
WAT34 1:35e874e10602 62
WAT34 1:35e874e10602 63 void boomerang::setTargetArmAngle(const int& targetArmAngle)
WAT34 1:35e874e10602 64 {
WAT34 2:aca690545ec9 65 armPID->setSetPoint(targetArmAngle);
WAT34 1:35e874e10602 66 }
WAT34 1:35e874e10602 67
WAT34 2:aca690545ec9 68 void boomerang::update()
WAT34 1:35e874e10602 69 {
WAT34 4:c2b09fa31492 70 if(!calibrating)
WAT34 4:c2b09fa31492 71 // armMotor->setSpeed(armPID->compute());
WAT34 4:c2b09fa31492 72 // shootingMotor->setSpeed(speedPID->compute());
WAT34 4:c2b09fa31492 73 beltMotor->setSpeed(beltSpeed);
WAT34 1:35e874e10602 74 }
WAT34 1:35e874e10602 75
WAT34 2:aca690545ec9 76 bool boomerang::fire()
WAT34 1:35e874e10602 77 {
WAT34 4:c2b09fa31492 78 if(firing)
WAT34 4:c2b09fa31492 79 return false;
WAT34 4:c2b09fa31492 80 firing = true;
WAT34 4:c2b09fa31492 81 beltSpeed=0.4;
WAT34 4:c2b09fa31492 82 beltTime.attach(callback(this,&boomerang::beltStop),0.5);
WAT34 2:aca690545ec9 83 spiral->rotate();
WAT34 4:c2b09fa31492 84 boomerang::update();
WAT34 1:35e874e10602 85
WAT34 2:aca690545ec9 86 return true;
WAT34 1:35e874e10602 87 }
WAT34 1:35e874e10602 88
WAT34 1:35e874e10602 89 void boomerang::beltStop()
WAT34 1:35e874e10602 90 {
WAT34 4:c2b09fa31492 91 beltSpeed = 0;
WAT34 4:c2b09fa31492 92 firing = false;
WAT34 4:c2b09fa31492 93 boomerang::update();
WAT34 4:c2b09fa31492 94
WAT34 1:35e874e10602 95 }
WAT34 1:35e874e10602 96
WAT34 1:35e874e10602 97 int boomerang::calibrateArm()
WAT34 1:35e874e10602 98 {
WAT34 2:aca690545ec9 99 if(armLimit->read() == true)
WAT34 1:35e874e10602 100 return 1;
WAT34 1:35e874e10602 101
WAT34 4:c2b09fa31492 102 calibrating = true;
WAT34 2:aca690545ec9 103 armLimit->enable_irq();
WAT34 2:aca690545ec9 104 armMotor->setSpeed(0.2);
WAT34 1:35e874e10602 105 return 0;
WAT34 1:35e874e10602 106 }
WAT34 1:35e874e10602 107
WAT34 1:35e874e10602 108 void boomerang::endCalibration()
WAT34 1:35e874e10602 109 {
WAT34 4:c2b09fa31492 110 calibrating = false;
WAT34 2:aca690545ec9 111 armEncoder->reset();
WAT34 2:aca690545ec9 112 armLimit->disable_irq();
WAT34 2:aca690545ec9 113 armPID->setSetPoint(10);
WAT34 1:35e874e10602 114 }