2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
Diff: hayatoBoomerang.cpp
- Revision:
- 12:9d21189a8eb0
- Parent:
- 11:68defd561031
- Child:
- 13:d01356a29b93
diff -r 68defd561031 -r 9d21189a8eb0 hayatoBoomerang.cpp --- a/hayatoBoomerang.cpp Fri Sep 01 14:04:21 2017 +0900 +++ b/hayatoBoomerang.cpp Wed Sep 13 22:58:42 2017 +0700 @@ -4,23 +4,30 @@ boomerang::boomerang(Serial *serial_) { speedEncoder = new InterruptIn(SPEED_ENCODER); + armEncoder= new QEI(PB_15,PB_4,NC,624); + armLimit = new InterruptIn(NC); //armLimit->mode(PullUp); rscs = new DigitalOut(NC); speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE); + armPID = new PID(ARM_P,ARM_I,ARM_D,RATE); speedPID->setInputLimits(0.0,9000.0); speedPID->setOutputLimits(0,1.0); + armPID->setInputLimits(5,80); + armPID->setOutputLimits(-1.0,1.0); speedPID->setBias(0); speedPID->setMode(AUTO_MODE); speedPID->setSetPoint(0); + armPID->setBias(0); + armPID->setSetPoint(0); + armPID->setMode(AUTO_MODE); - armValve[0] = new DigitalOut(D13); - armValve[1] = new DigitalOut(D13); + armMotor = new ikarashiMDC(rscs,0,0,SM,serial_); shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_); spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_); beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_); @@ -35,8 +42,7 @@ omniMotor[2]->braking=true; omniMotor[3]->braking=true; - armValve[0]->write(false); - armValve[1]->write(false); + armMotor->setSpeed(0); shootingMotor->setSpeed(0); spiralMotor->setSpeed(0); beltMotor->setSpeed(0); @@ -45,10 +51,13 @@ motorRPM = 0; - omni->setWheelRadian(radians(45),radians(45+90),radians(45+180),radians(45+270)); + omni->setWheelRadian(radians(51.675),radians(76.65+51.675),radians(76.65+51.675+103.35),radians(360-51.675)); speedEncoder->rise(this,&boomerang::shootingEncoderCount); speedEncoder->enable_irq(); + armLimit->fall(this,&boomerang::endCalibration); + armLimit->disable_irq(); + firing = false; boomerang::update(); @@ -74,10 +83,18 @@ speedPID->setSetPoint(targetRPM); } +void boomerang::setTargetArmAngle(const int& targetArmAngle) +{ + armPID->setSetPoint(targetArmAngle); +} void boomerang::update() { + armAngle = armEncoder->getPulses()/800.0*85.51; + armPID->setProcessValue(armAngle); speedPID->setProcessValue(motorRPM); + if(!calibrating) + armMotor->setSpeed(armPID->compute()); shootingMotor->setSpeed(speedPID->compute()); beltMotor->setSpeed(beltSpeed); } @@ -102,8 +119,21 @@ } -void boomerang::sword() +int boomerang::calibrateArm() { - armValve[0]->write(!armValve[0]->read()); - armValve[1]->write(!armValve[0]->read()); + 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); +}