hayatoBoomerang.cpp

00001 #include "hayatoBoomerang.h"
00002 #include "pinConfig.h"
00003 
00004 boomerang::boomerang(Serial *serial_,Serial *serial2_)
00005 {
00006   spiralSerial = serial2_;
00007   speedEncoder = new InterruptIn(SPEED_ENCODER);
00008   armEncoder= new QEI(PB_15,PB_4,NC,624);
00009 
00010   armLimit = new InterruptIn(NC);
00011   //armLimit->mode(PullUp);
00012 
00013   rscs = new DigitalOut(NC);
00014 
00015   speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE);
00016   armPID = new PID(ARM_P,ARM_I,ARM_D,RATE);
00017   speedPID->setInputLimits(0.0,15000.0);
00018   speedPID->setOutputLimits(0,1.0);
00019 
00020   armPID->setInputLimits(0,81);
00021   armPID->setOutputLimits(-1.0,1.0);
00022 
00023   speedPID->setBias(0);
00024   speedPID->setMode(AUTO_MODE);
00025   speedPID->setSetPoint(0);
00026 
00027   armPID->setBias(0);
00028   armPID->setSetPoint(0);
00029   armPID->setMode(AUTO_MODE);
00030 
00031   armMotor = new ikarashiMDC(rscs,0,0,SM,serial_);
00032   shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
00033   spiralMotor = new ikarashiMDC(rscs,0,0,SM,spiralSerial);
00034   beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
00035 
00036   omni = new Omni(4);
00037   omniMotor[0] = new ikarashiMDC(rscs,1,0,SM,serial_);
00038   omniMotor[1] = new ikarashiMDC(rscs,1,1,SM,serial_);
00039   omniMotor[2] = new ikarashiMDC(rscs,1,2,SM,serial_);
00040   omniMotor[3] = new ikarashiMDC(rscs,1,3,SM,serial_);
00041 
00042   omniMotor[0]->braking=true;
00043   omniMotor[1]->braking=true;
00044   omniMotor[2]->braking=true;
00045   omniMotor[3]->braking=true;
00046 
00047   shootingMotor->braking=false;
00048 
00049   spiralMotor->braking=true;
00050 
00051   armMotor->setSpeed(0);
00052   shootingMotor->setSpeed(0);
00053   spiralMotor->setSpeed(0);
00054   beltMotor->setSpeed(0);
00055   omniMotor[0]->setSpeed(0);
00056   omniMotor[1]->setSpeed(0);
00057   omniMotor[2]->setSpeed(0);
00058   omniMotor[3]->setSpeed(0);
00059 
00060   spiral = new Spiral(spiralMotor);
00061 
00062   motorRPM = 0;
00063 
00064   omni->setWheelRadian(radians(51.675),radians(76.65+51.675),radians(76.65+51.675+103.35),radians(360-51.675));
00065 
00066   speedEncoder->rise(this,&boomerang::shootingEncoderCount);
00067   speedEncoder->enable_irq();
00068 
00069 
00070 }
00071 
00072 void boomerang::move(const uint8_t& x,const uint8_t& y,const float& moment)
00073 {
00074     omni->computeXY((y-127)/130.0,(x-127)/130.0,moment);
00075     for (size_t i = 0; i < 4; i++) {
00076       omniMotor[i]->setSpeed(omni->getOutput(i));
00077     }
00078 }
00079 
00080 void boomerang::shootingEncoderCount()
00081 {
00082   motorRPM = (1.0/rpmCalc.read())*60;
00083   rpmCalc.reset();
00084   rpmCalc.start();
00085 }
00086 
00087 void boomerang::setTargetMotorSpeed(const int& targetRPM)
00088 {
00089   if(targetRPM == 0){
00090     speedPID->reset();
00091     speedPID->setSetPoint(0);
00092     return;
00093   }
00094   speedPID->setSetPoint(targetRPM);
00095 }
00096 
00097 void boomerang::setTargetArmAngle(const int& targetArmAngle)
00098 {
00099     armPID->setSetPoint(targetArmAngle);
00100 }
00101 
00102 void boomerang::update()
00103 {
00104   armAngle = armEncoder->getPulses()/800.0*85.51;
00105   armPID->setProcessValue(armAngle);
00106   speedPID->setProcessValue(motorRPM);
00107   armMotor->setSpeed(armPID->compute());
00108   shootingMotor->setSpeed(speedPID->compute());
00109   beltMotor->setSpeed(spiral->beltSpeed);
00110   spiralMotor->setSpeed(spiral->spiralSpeed);
00111 }
00112 
00113 bool boomerang::fire()
00114 {
00115   if(spiral->firing)
00116     return false;
00117   spiral->rotate();
00118   return true;
00119 }