NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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

Dependencies:   PID QEI ikarashiMDC recieveController omni

hayatoBoomerang.cpp@13:d01356a29b93, 2017-10-05 (annotated)

Committer:
WAT34
Date:
Thu Oct 05 16:04:46 2017 +0900
Revision:
13:d01356a29b93
Parent:
12:9d21189a8eb0
Child:
14:5cc44bec9cfc
omniwheel bug fixed

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 1:35e874e10602 4 boomerang::boomerang(Serial *serial_)
WAT34 1:35e874e10602 5 {
WAT34 2:aca690545ec9 6 speedEncoder = new InterruptIn(SPEED_ENCODER);
WAT34 12:9d21189a8eb0 7 armEncoder= new QEI(PB_15,PB_4,NC,624);
WAT34 1:35e874e10602 8
WAT34 12:9d21189a8eb0 9 armLimit = new InterruptIn(NC);
WAT34 4:c2b09fa31492 10 //armLimit->mode(PullUp);
WAT34 2:aca690545ec9 11
WAT34 2:aca690545ec9 12 rscs = new DigitalOut(NC);
WAT34 1:35e874e10602 13
WAT34 1:35e874e10602 14 speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE);
WAT34 12:9d21189a8eb0 15 armPID = new PID(ARM_P,ARM_I,ARM_D,RATE);
WAT34 13:d01356a29b93 16 speedPID->setInputLimits(0.0,15000.0);
WAT34 2:aca690545ec9 17 speedPID->setOutputLimits(0,1.0);
WAT34 1:35e874e10602 18
WAT34 12:9d21189a8eb0 19 armPID->setInputLimits(5,80);
WAT34 12:9d21189a8eb0 20 armPID->setOutputLimits(-1.0,1.0);
WAT34 2:aca690545ec9 21
WAT34 2:aca690545ec9 22 speedPID->setBias(0);
WAT34 2:aca690545ec9 23 speedPID->setMode(AUTO_MODE);
WAT34 2:aca690545ec9 24 speedPID->setSetPoint(0);
WAT34 1:35e874e10602 25
WAT34 12:9d21189a8eb0 26 armPID->setBias(0);
WAT34 12:9d21189a8eb0 27 armPID->setSetPoint(0);
WAT34 12:9d21189a8eb0 28 armPID->setMode(AUTO_MODE);
WAT34 1:35e874e10602 29
WAT34 12:9d21189a8eb0 30 armMotor = new ikarashiMDC(rscs,0,0,SM,serial_);
WAT34 2:aca690545ec9 31 shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
WAT34 2:aca690545ec9 32 spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
WAT34 2:aca690545ec9 33 beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
WAT34 1:35e874e10602 34
WAT34 6:0bee4b2bb400 35 omni = new Omni(4);
WAT34 6:0bee4b2bb400 36 omniMotor[0] = new ikarashiMDC(rscs,1,0,SM,serial_);
WAT34 6:0bee4b2bb400 37 omniMotor[1] = new ikarashiMDC(rscs,1,1,SM,serial_);
WAT34 6:0bee4b2bb400 38 omniMotor[2] = new ikarashiMDC(rscs,1,2,SM,serial_);
WAT34 6:0bee4b2bb400 39 omniMotor[3] = new ikarashiMDC(rscs,1,3,SM,serial_);
WAT34 13:d01356a29b93 40
WAT34 6:0bee4b2bb400 41 omniMotor[0]->braking=true;
WAT34 6:0bee4b2bb400 42 omniMotor[1]->braking=true;
WAT34 6:0bee4b2bb400 43 omniMotor[2]->braking=true;
WAT34 6:0bee4b2bb400 44 omniMotor[3]->braking=true;
WAT34 6:0bee4b2bb400 45
WAT34 12:9d21189a8eb0 46 armMotor->setSpeed(0);
WAT34 4:c2b09fa31492 47 shootingMotor->setSpeed(0);
WAT34 4:c2b09fa31492 48 spiralMotor->setSpeed(0);
WAT34 4:c2b09fa31492 49 beltMotor->setSpeed(0);
WAT34 13:d01356a29b93 50 omniMotor[0]->setSpeed(0);
WAT34 13:d01356a29b93 51 omniMotor[1]->setSpeed(0);
WAT34 13:d01356a29b93 52 omniMotor[2]->setSpeed(0);
WAT34 13:d01356a29b93 53 omniMotor[3]->setSpeed(0);
WAT34 4:c2b09fa31492 54
WAT34 2:aca690545ec9 55 spiral = new Spiral(spiralMotor);
WAT34 1:35e874e10602 56
WAT34 1:35e874e10602 57 motorRPM = 0;
WAT34 1:35e874e10602 58
WAT34 12:9d21189a8eb0 59 omni->setWheelRadian(radians(51.675),radians(76.65+51.675),radians(76.65+51.675+103.35),radians(360-51.675));
WAT34 6:0bee4b2bb400 60
WAT34 2:aca690545ec9 61 speedEncoder->rise(this,&boomerang::shootingEncoderCount);
WAT34 2:aca690545ec9 62 speedEncoder->enable_irq();
WAT34 12:9d21189a8eb0 63 armLimit->fall(this,&boomerang::endCalibration);
WAT34 12:9d21189a8eb0 64 armLimit->disable_irq();
WAT34 12:9d21189a8eb0 65
WAT34 6:0bee4b2bb400 66 firing = false;
WAT34 6:0bee4b2bb400 67
WAT34 6:0bee4b2bb400 68 boomerang::update();
WAT34 6:0bee4b2bb400 69 }
WAT34 6:0bee4b2bb400 70
WAT34 6:0bee4b2bb400 71 void boomerang::move(const uint8_t& x,const uint8_t& y,const uint8_t& moment)
WAT34 6:0bee4b2bb400 72 {
WAT34 13:d01356a29b93 73 omni->computeXY((y-127)/130.0,(x-127)/130.0,moment);
WAT34 6:0bee4b2bb400 74 for (size_t i = 0; i < 4; i++) {
WAT34 6:0bee4b2bb400 75 omniMotor[i]->setSpeed((omni->getOutput(i)-127)/127.0);
WAT34 6:0bee4b2bb400 76 }
WAT34 1:35e874e10602 77 }
WAT34 1:35e874e10602 78
WAT34 1:35e874e10602 79 void boomerang::shootingEncoderCount()
WAT34 1:35e874e10602 80 {
WAT34 1:35e874e10602 81 motorRPM = (1.0/rpmCalc.read())*60;
WAT34 1:35e874e10602 82 rpmCalc.reset();
WAT34 1:35e874e10602 83 rpmCalc.start();
WAT34 1:35e874e10602 84 }
WAT34 1:35e874e10602 85
WAT34 1:35e874e10602 86 void boomerang::setTargetMotorSpeed(const int& targetRPM)
WAT34 1:35e874e10602 87 {
WAT34 2:aca690545ec9 88 speedPID->setSetPoint(targetRPM);
WAT34 1:35e874e10602 89 }
WAT34 1:35e874e10602 90
WAT34 12:9d21189a8eb0 91 void boomerang::setTargetArmAngle(const int& targetArmAngle)
WAT34 12:9d21189a8eb0 92 {
WAT34 12:9d21189a8eb0 93 armPID->setSetPoint(targetArmAngle);
WAT34 12:9d21189a8eb0 94 }
WAT34 1:35e874e10602 95
WAT34 2:aca690545ec9 96 void boomerang::update()
WAT34 1:35e874e10602 97 {
WAT34 12:9d21189a8eb0 98 armAngle = armEncoder->getPulses()/800.0*85.51;
WAT34 12:9d21189a8eb0 99 armPID->setProcessValue(armAngle);
WAT34 6:0bee4b2bb400 100 speedPID->setProcessValue(motorRPM);
WAT34 12:9d21189a8eb0 101 if(!calibrating)
WAT34 12:9d21189a8eb0 102 armMotor->setSpeed(armPID->compute());
WAT34 6:0bee4b2bb400 103 shootingMotor->setSpeed(speedPID->compute());
WAT34 4:c2b09fa31492 104 beltMotor->setSpeed(beltSpeed);
WAT34 13:d01356a29b93 105
WAT34 1:35e874e10602 106 }
WAT34 1:35e874e10602 107
WAT34 2:aca690545ec9 108 bool boomerang::fire()
WAT34 1:35e874e10602 109 {
WAT34 4:c2b09fa31492 110 if(firing)
WAT34 4:c2b09fa31492 111 return false;
WAT34 4:c2b09fa31492 112 firing = true;
WAT34 4:c2b09fa31492 113 beltSpeed=0.4;
WAT34 6:0bee4b2bb400 114 beltTime.attach(callback(this,&boomerang::beltStop),1);
WAT34 2:aca690545ec9 115 spiral->rotate();
WAT34 4:c2b09fa31492 116 boomerang::update();
WAT34 2:aca690545ec9 117 return true;
WAT34 1:35e874e10602 118 }
WAT34 1:35e874e10602 119
WAT34 1:35e874e10602 120 void boomerang::beltStop()
WAT34 1:35e874e10602 121 {
WAT34 4:c2b09fa31492 122 beltSpeed = 0;
WAT34 4:c2b09fa31492 123 firing = false;
WAT34 4:c2b09fa31492 124 boomerang::update();
WAT34 4:c2b09fa31492 125
WAT34 1:35e874e10602 126 }
WAT34 1:35e874e10602 127
WAT34 12:9d21189a8eb0 128 int boomerang::calibrateArm()
WAT34 1:35e874e10602 129 {
WAT34 12:9d21189a8eb0 130 if(armLimit->read() == true)
WAT34 12:9d21189a8eb0 131 return 1;
WAT34 12:9d21189a8eb0 132
WAT34 12:9d21189a8eb0 133 calibrating = true;
WAT34 12:9d21189a8eb0 134 armLimit->enable_irq();
WAT34 12:9d21189a8eb0 135 armMotor->setSpeed(0.2);
WAT34 12:9d21189a8eb0 136 return 0;
WAT34 1:35e874e10602 137 }
WAT34 12:9d21189a8eb0 138
WAT34 12:9d21189a8eb0 139 void boomerang::endCalibration()
WAT34 12:9d21189a8eb0 140 {
WAT34 12:9d21189a8eb0 141 calibrating = false;
WAT34 12:9d21189a8eb0 142 armEncoder->reset();
WAT34 12:9d21189a8eb0 143 armLimit->disable_irq();
WAT34 12:9d21189a8eb0 144 armPID->setSetPoint(10);
WAT34 12:9d21189a8eb0 145 }