NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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

Dependencies:   PID QEI ikarashiMDC recieveController omni

hayatoBoomerang.cpp@10:5ef1515735a4, 2017-09-13 (annotated)

Committer:
Litvyak_eil4 ]
Date:
Wed Sep 13 22:55:31 2017 +0900
Revision:
10:5ef1515735a4
Parent:
9:6540a10a07fb
Child:
14:5cc44bec9cfc
welcome back my honey wataru

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