2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
Diff: hayatoBoomerang.cpp
- Revision:
- 6:0bee4b2bb400
- Parent:
- 4:c2b09fa31492
- Child:
- 8:c47cf4e0230c
- Child:
- 11:68defd561031
diff -r 9c52e6c14c87 -r 0bee4b2bb400 hayatoBoomerang.cpp --- a/hayatoBoomerang.cpp Wed Aug 30 03:44:00 2017 +0900 +++ b/hayatoBoomerang.cpp Fri Sep 01 10:42:27 2017 +0900 @@ -4,7 +4,7 @@ boomerang::boomerang(Serial *serial_) { speedEncoder = new InterruptIn(SPEED_ENCODER); - armEncoder= new QEI(NC,NC,NC,720); + armEncoder= new QEI(D10,D11,NC,624); armLimit = new InterruptIn(NC); //armLimit->mode(PullUp); @@ -13,11 +13,11 @@ speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE); armPID = new PID(ARM_P,ARM_I,ARM_D,RATE); - speedPID->setInputLimits(0.0,6000.0); + speedPID->setInputLimits(0.0,9000.0); speedPID->setOutputLimits(0,1.0); - armPID->setInputLimits(0,90); - speedPID->setOutputLimits(-1.0,1.0); + armPID->setInputLimits(5,80); + armPID->setOutputLimits(-1.0,1.0); speedPID->setBias(0); speedPID->setMode(AUTO_MODE); @@ -32,6 +32,16 @@ spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_); beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_); + omni = new Omni(4); + omniMotor[0] = new ikarashiMDC(rscs,1,0,SM,serial_); + omniMotor[1] = new ikarashiMDC(rscs,1,1,SM,serial_); + omniMotor[2] = new ikarashiMDC(rscs,1,2,SM,serial_); + omniMotor[3] = new ikarashiMDC(rscs,1,3,SM,serial_); + omniMotor[0]->braking=true; + omniMotor[1]->braking=true; + omniMotor[2]->braking=true; + omniMotor[3]->braking=true; + armMotor->setSpeed(0); shootingMotor->setSpeed(0); spiralMotor->setSpeed(0); @@ -41,11 +51,24 @@ motorRPM = 0; + 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(); +} + +void boomerang::move(const uint8_t& x,const uint8_t& y,const uint8_t& moment) +{ + omni->computeXY((y-127)/127.0,(x-127)/127.0,moment); + for (size_t i = 0; i < 4; i++) { + omniMotor[i]->setSpeed((omni->getOutput(i)-127)/127.0); + } } void boomerang::shootingEncoderCount() @@ -67,9 +90,12 @@ 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()); + armMotor->setSpeed(armPID->compute()); + shootingMotor->setSpeed(speedPID->compute()); beltMotor->setSpeed(beltSpeed); } @@ -79,10 +105,9 @@ return false; firing = true; beltSpeed=0.4; - beltTime.attach(callback(this,&boomerang::beltStop),0.5); + beltTime.attach(callback(this,&boomerang::beltStop),1); spiral->rotate(); boomerang::update(); - return true; }