2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
Diff: hayatoBoomerang.cpp
- Revision:
- 17:311aed3cad15
- Parent:
- 16:5f205692b14d
- Child:
- 18:4b629221c215
diff -r 5f205692b14d -r 311aed3cad15 hayatoBoomerang.cpp --- a/hayatoBoomerang.cpp Fri Oct 06 21:59:16 2017 +0900 +++ b/hayatoBoomerang.cpp Sun Oct 15 13:42:12 2017 +0900 @@ -1,8 +1,9 @@ #include "hayatoBoomerang.h" #include "pinConfig.h" -boomerang::boomerang(Serial *serial_) +boomerang::boomerang(Serial *serial_,Serial *serial2_) { + spiralSerial = serial2_; speedEncoder = new InterruptIn(SPEED_ENCODER); armEncoder= new QEI(PB_15,PB_4,NC,624); @@ -29,7 +30,7 @@ armMotor = new ikarashiMDC(rscs,0,0,SM,serial_); shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_); - spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_); + spiralMotor = new ikarashiMDC(rscs,0,0,SM,spiralSerial); beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_); omni = new Omni(4); @@ -43,6 +44,10 @@ omniMotor[2]->braking=true; omniMotor[3]->braking=true; + shootingMotor->braking=false; + + spiralMotor->braking=true; + armMotor->setSpeed(0); shootingMotor->setSpeed(0); spiralMotor->setSpeed(0); @@ -60,12 +65,8 @@ speedEncoder->rise(this,&boomerang::shootingEncoderCount); speedEncoder->enable_irq(); - armLimit->fall(this,&boomerang::endCalibration); - armLimit->disable_irq(); - firing = false; - boomerang::update(); } void boomerang::move(const uint8_t& x,const uint8_t& y,const uint8_t& moment) @@ -85,6 +86,11 @@ void boomerang::setTargetMotorSpeed(const int& targetRPM) { + if(targetRPM == 0){ + speedPID->reset(); + speedPID->setSetPoint(0); + return; + } speedPID->setSetPoint(targetRPM); } @@ -98,48 +104,16 @@ armAngle = armEncoder->getPulses()/800.0*85.51; armPID->setProcessValue(armAngle); speedPID->setProcessValue(motorRPM); - if(!calibrating) - armMotor->setSpeed(armPID->compute()); + armMotor->setSpeed(armPID->compute()); shootingMotor->setSpeed(speedPID->compute()); - beltMotor->setSpeed(beltSpeed); - + beltMotor->setSpeed(spiral->beltSpeed); + spiralMotor->setSpeed(spiral->spiralSpeed); } bool boomerang::fire() { - if(firing) + if(spiral->firing) return false; - firing = true; - beltSpeed=0.4; - beltTime.attach(callback(this,&boomerang::beltStop),1); spiral->rotate(); - boomerang::update(); return true; } - -void boomerang::beltStop() -{ - beltSpeed = 0; - firing = false; - boomerang::update(); - -} - -int boomerang::calibrateArm() -{ - if(armLimit->read() == true) - return 1; - - calibrating = true; - armLimit->enable_irq(); - armMotor->setSpeed(0.2); - return 0; -} - -void boomerang::endCalibration() -{ - calibrating = false; - armEncoder->reset(); - armLimit->disable_irq(); - armPID->setSetPoint(10); -}