2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
hayatoBoomerang.cpp
- Committer:
- WAT34
- Date:
- 2017-11-13
- Revision:
- 21:aad4a4adc18c
- Parent:
- 18:4b629221c215
File content as of revision 21:aad4a4adc18c:
#include "hayatoBoomerang.h" #include "pinConfig.h" boomerang::boomerang(Serial *serial_,Serial *serial2_) { spiralSerial = serial2_; 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,15000.0); speedPID->setOutputLimits(0,1.0); armPID->setInputLimits(0,81); 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); armMotor = new ikarashiMDC(rscs,0,0,SM,serial_); shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_); spiralMotor = new ikarashiMDC(rscs,0,0,SM,spiralSerial); 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; shootingMotor->braking=false; spiralMotor->braking=true; armMotor->setSpeed(0); shootingMotor->setSpeed(0); spiralMotor->setSpeed(0); beltMotor->setSpeed(0); omniMotor[0]->setSpeed(0); omniMotor[1]->setSpeed(0); omniMotor[2]->setSpeed(0); omniMotor[3]->setSpeed(0); spiral = new Spiral(spiralMotor); 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(); } void boomerang::move(const uint8_t& x,const uint8_t& y,const float& moment) { omni->computeXY((y-127)/130.0,(x-127)/130.0,moment); for (size_t i = 0; i < 4; i++) { omniMotor[i]->setSpeed(omni->getOutput(i)); } } void boomerang::shootingEncoderCount() { motorRPM = (1.0/rpmCalc.read())*60; rpmCalc.reset(); rpmCalc.start(); } void boomerang::setTargetMotorSpeed(const int& targetRPM) { if(targetRPM == 0){ speedPID->reset(); speedPID->setSetPoint(0); return; } 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); armMotor->setSpeed(armPID->compute()); shootingMotor->setSpeed(speedPID->compute()); beltMotor->setSpeed(spiral->beltSpeed); spiralMotor->setSpeed(spiral->spiralSpeed); } bool boomerang::fire() { if(spiral->firing) return false; spiral->beltStart(); return true; }