NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

2017ロボコンはやとブーメランプログラム

Dependencies:   PID QEI ikarashiMDC recieveController omni

hayatoBoomerang.cpp@20:d052a0679309, 2017-11-06 (annotated)

Committer:
WAT34
Date:
Mon Nov 06 18:54:45 2017 +0900
Revision:
20:d052a0679309
Parent:
18:4b629221c215 
belt pided

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WAT34 1:35e874e10602 1 #include "hayatoBoomerang.h"
WAT34 2:aca690545ec9 2 #include "pinConfig.h"
WAT34 1:35e874e10602 3
WAT34 17:311aed3cad15 4 boomerang::boomerang(Serial *serial_,Serial *serial2_)
WAT34 1:35e874e10602 5 {
WAT34 17:311aed3cad15 6 spiralSerial = serial2_;
WAT34 2:aca690545ec9 7 speedEncoder = new InterruptIn(SPEED_ENCODER);
Litvyak_eil4 9:6540a10a07fb 8 armEncoder= new QEI(PB_15,PB_4,NC,624);
WAT34 1:35e874e10602 9
WAT34 4:c2b09fa31492 10 armLimit = new InterruptIn(NC);
WAT34 4:c2b09fa31492 11 //armLimit->mode(PullUp);
WAT34 2:aca690545ec9 12
WAT34 2:aca690545ec9 13 rscs = new DigitalOut(NC);
WAT34 1:35e874e10602 14
WAT34 1:35e874e10602 15 speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE);
WAT34 1:35e874e10602 16 armPID = new PID(ARM_P,ARM_I,ARM_D,RATE);
WAT34 13:d01356a29b93 17 speedPID->setInputLimits(0.0,15000.0);
WAT34 2:aca690545ec9 18 speedPID->setOutputLimits(0,1.0);
WAT34 1:35e874e10602 19
WAT34 16:5f205692b14d 20 armPID->setInputLimits(0,81);
WAT34 6:0bee4b2bb400 21 armPID->setOutputLimits(-1.0,1.0);
WAT34 2:aca690545ec9 22
WAT34 2:aca690545ec9 23 speedPID->setBias(0);
WAT34 2:aca690545ec9 24 speedPID->setMode(AUTO_MODE);
WAT34 2:aca690545ec9 25 speedPID->setSetPoint(0);
WAT34 1:35e874e10602 26
WAT34 2:aca690545ec9 27 armPID->setBias(0);
WAT34 2:aca690545ec9 28 armPID->setSetPoint(0);
WAT34 2:aca690545ec9 29 armPID->setMode(AUTO_MODE);
WAT34 1:35e874e10602 30
WAT34 2:aca690545ec9 31 armMotor = new ikarashiMDC(rscs,0,0,SM,serial_);
WAT34 2:aca690545ec9 32 shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
WAT34 17:311aed3cad15 33 spiralMotor = new ikarashiMDC(rscs,0,0,SM,spiralSerial);
WAT34 2:aca690545ec9 34 beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
WAT34 1:35e874e10602 35
WAT34 6:0bee4b2bb400 36 omni = new Omni(4);
WAT34 6:0bee4b2bb400 37 omniMotor[0] = new ikarashiMDC(rscs,1,0,SM,serial_);
WAT34 6:0bee4b2bb400 38 omniMotor[1] = new ikarashiMDC(rscs,1,1,SM,serial_);
WAT34 6:0bee4b2bb400 39 omniMotor[2] = new ikarashiMDC(rscs,1,2,SM,serial_);
WAT34 6:0bee4b2bb400 40 omniMotor[3] = new ikarashiMDC(rscs,1,3,SM,serial_);
WAT34 13:d01356a29b93 41
WAT34 6:0bee4b2bb400 42 omniMotor[0]->braking=true;
WAT34 6:0bee4b2bb400 43 omniMotor[1]->braking=true;
WAT34 6:0bee4b2bb400 44 omniMotor[2]->braking=true;
WAT34 6:0bee4b2bb400 45 omniMotor[3]->braking=true;
WAT34 6:0bee4b2bb400 46
WAT34 17:311aed3cad15 47 shootingMotor->braking=false;
WAT34 17:311aed3cad15 48
WAT34 17:311aed3cad15 49 spiralMotor->braking=true;
WAT34 17:311aed3cad15 50
WAT34 4:c2b09fa31492 51 armMotor->setSpeed(0);
WAT34 4:c2b09fa31492 52 shootingMotor->setSpeed(0);
WAT34 4:c2b09fa31492 53 spiralMotor->setSpeed(0);
WAT34 4:c2b09fa31492 54 beltMotor->setSpeed(0);
WAT34 13:d01356a29b93 55 omniMotor[0]->setSpeed(0);
WAT34 13:d01356a29b93 56 omniMotor[1]->setSpeed(0);
WAT34 13:d01356a29b93 57 omniMotor[2]->setSpeed(0);
WAT34 13:d01356a29b93 58 omniMotor[3]->setSpeed(0);
WAT34 4:c2b09fa31492 59
WAT34 2:aca690545ec9 60 spiral = new Spiral(spiralMotor);
WAT34 1:35e874e10602 61
WAT34 1:35e874e10602 62 motorRPM = 0;
WAT34 1:35e874e10602 63
WAT34 6:0bee4b2bb400 64 omni->setWheelRadian(radians(51.675),radians(76.65+51.675),radians(76.65+51.675+103.35),radians(360-51.675));
WAT34 6:0bee4b2bb400 65
WAT34 2:aca690545ec9 66 speedEncoder->rise(this,&boomerang::shootingEncoderCount);
WAT34 2:aca690545ec9 67 speedEncoder->enable_irq();
WAT34 1:35e874e10602 68
WAT34 6:0bee4b2bb400 69
WAT34 6:0bee4b2bb400 70 }
WAT34 6:0bee4b2bb400 71
WAT34 18:4b629221c215 72 void boomerang::move(const uint8_t& x,const uint8_t& y,const float& moment)
WAT34 6:0bee4b2bb400 73 {
WAT34 13:d01356a29b93 74 omni->computeXY((y-127)/130.0,(x-127)/130.0,moment);
WAT34 6:0bee4b2bb400 75 for (size_t i = 0; i < 4; i++) {
WAT34 18:4b629221c215 76 omniMotor[i]->setSpeed(omni->getOutput(i));
WAT34 6:0bee4b2bb400 77 }
WAT34 1:35e874e10602 78 }
WAT34 1:35e874e10602 79
WAT34 1:35e874e10602 80 void boomerang::shootingEncoderCount()
WAT34 1:35e874e10602 81 {
WAT34 1:35e874e10602 82 motorRPM = (1.0/rpmCalc.read())*60;
WAT34 1:35e874e10602 83 rpmCalc.reset();
WAT34 1:35e874e10602 84 rpmCalc.start();
WAT34 1:35e874e10602 85 }
WAT34 1:35e874e10602 86
WAT34 1:35e874e10602 87 void boomerang::setTargetMotorSpeed(const int& targetRPM)
WAT34 1:35e874e10602 88 {
WAT34 17:311aed3cad15 89 if(targetRPM == 0){
WAT34 17:311aed3cad15 90 speedPID->reset();
WAT34 17:311aed3cad15 91 speedPID->setSetPoint(0);
WAT34 17:311aed3cad15 92 return;
WAT34 17:311aed3cad15 93 }
WAT34 2:aca690545ec9 94 speedPID->setSetPoint(targetRPM);
WAT34 1:35e874e10602 95 }
WAT34 1:35e874e10602 96
WAT34 1:35e874e10602 97 void boomerang::setTargetArmAngle(const int& targetArmAngle)
WAT34 1:35e874e10602 98 {
WAT34 2:aca690545ec9 99 armPID->setSetPoint(targetArmAngle);
WAT34 1:35e874e10602 100 }
WAT34 1:35e874e10602 101
WAT34 2:aca690545ec9 102 void boomerang::update()
WAT34 1:35e874e10602 103 {
WAT34 20:d052a0679309 104 spiral->update();
WAT34 6:0bee4b2bb400 105 armAngle = armEncoder->getPulses()/800.0*85.51;
WAT34 6:0bee4b2bb400 106 armPID->setProcessValue(armAngle);
WAT34 6:0bee4b2bb400 107 speedPID->setProcessValue(motorRPM);
WAT34 17:311aed3cad15 108 armMotor->setSpeed(armPID->compute());
WAT34 6:0bee4b2bb400 109 shootingMotor->setSpeed(speedPID->compute());
WAT34 17:311aed3cad15 110 beltMotor->setSpeed(spiral->beltSpeed);
WAT34 17:311aed3cad15 111 spiralMotor->setSpeed(spiral->spiralSpeed);
WAT34 20:d052a0679309 112 //printf("%d %f\n",spiral->beltEncoder->getPulses(),spiral->beltSpeed);
WAT34 1:35e874e10602 113 }
WAT34 1:35e874e10602 114
WAT34 2:aca690545ec9 115 bool boomerang::fire()
WAT34 1:35e874e10602 116 {
WAT34 17:311aed3cad15 117 if(spiral->firing)
WAT34 4:c2b09fa31492 118 return false;
WAT34 2:aca690545ec9 119 spiral->rotate();
WAT34 2:aca690545ec9 120 return true;
WAT34 1:35e874e10602 121 }