2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
hayatoBoomerang.cpp
- Committer:
- WAT34
- Date:
- 2017-09-01
- Revision:
- 11:68defd561031
- Parent:
- 6:0bee4b2bb400
- Child:
- 12:9d21189a8eb0
File content as of revision 11:68defd561031:
#include "hayatoBoomerang.h" #include "pinConfig.h" boomerang::boomerang(Serial *serial_) { speedEncoder = new InterruptIn(SPEED_ENCODER); //armLimit->mode(PullUp); rscs = new DigitalOut(NC); speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE); speedPID->setInputLimits(0.0,9000.0); speedPID->setOutputLimits(0,1.0); speedPID->setBias(0); speedPID->setMode(AUTO_MODE); speedPID->setSetPoint(0); armValve[0] = new DigitalOut(D13); armValve[1] = new DigitalOut(D13); shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_); 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; armValve[0]->write(false); armValve[1]->write(false); shootingMotor->setSpeed(0); spiralMotor->setSpeed(0); beltMotor->setSpeed(0); spiral = new Spiral(spiralMotor); motorRPM = 0; omni->setWheelRadian(radians(45),radians(45+90),radians(45+180),radians(45+270)); speedEncoder->rise(this,&boomerang::shootingEncoderCount); speedEncoder->enable_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() { motorRPM = (1.0/rpmCalc.read())*60; rpmCalc.reset(); rpmCalc.start(); } void boomerang::setTargetMotorSpeed(const int& targetRPM) { speedPID->setSetPoint(targetRPM); } void boomerang::update() { speedPID->setProcessValue(motorRPM); shootingMotor->setSpeed(speedPID->compute()); beltMotor->setSpeed(beltSpeed); } bool boomerang::fire() { if(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(); } void boomerang::sword() { armValve[0]->write(!armValve[0]->read()); armValve[1]->write(!armValve[0]->read()); }