NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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

Dependencies:   PID QEI ikarashiMDC recieveController omni

hayatoBoomerang.cpp@12:9d21189a8eb0, 2017-09-13 (annotated)

Committer:
WAT34
Date:
Wed Sep 13 22:58:42 2017 +0700
Revision:
12:9d21189a8eb0
Parent:
11:68defd561031
Child:
13:d01356a29b93
pinconfig 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 6:0bee4b2bb400 16 speedPID->setInputLimits(0.0,9000.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 6:0bee4b2bb400 40 omniMotor[0]->braking=true;
WAT34 6:0bee4b2bb400 41 omniMotor[1]->braking=true;
WAT34 6:0bee4b2bb400 42 omniMotor[2]->braking=true;
WAT34 6:0bee4b2bb400 43 omniMotor[3]->braking=true;
WAT34 6:0bee4b2bb400 44
WAT34 12:9d21189a8eb0 45 armMotor->setSpeed(0);
WAT34 4:c2b09fa31492 46 shootingMotor->setSpeed(0);
WAT34 4:c2b09fa31492 47 spiralMotor->setSpeed(0);
WAT34 4:c2b09fa31492 48 beltMotor->setSpeed(0);
WAT34 4:c2b09fa31492 49
WAT34 2:aca690545ec9 50 spiral = new Spiral(spiralMotor);
WAT34 1:35e874e10602 51
WAT34 1:35e874e10602 52 motorRPM = 0;
WAT34 1:35e874e10602 53
WAT34 12:9d21189a8eb0 54 omni->setWheelRadian(radians(51.675),radians(76.65+51.675),radians(76.65+51.675+103.35),radians(360-51.675));
WAT34 6:0bee4b2bb400 55
WAT34 2:aca690545ec9 56 speedEncoder->rise(this,&boomerang::shootingEncoderCount);
WAT34 2:aca690545ec9 57 speedEncoder->enable_irq();
WAT34 12:9d21189a8eb0 58 armLimit->fall(this,&boomerang::endCalibration);
WAT34 12:9d21189a8eb0 59 armLimit->disable_irq();
WAT34 12:9d21189a8eb0 60
WAT34 6:0bee4b2bb400 61 firing = false;
WAT34 6:0bee4b2bb400 62
WAT34 6:0bee4b2bb400 63 boomerang::update();
WAT34 6:0bee4b2bb400 64 }
WAT34 6:0bee4b2bb400 65
WAT34 6:0bee4b2bb400 66 void boomerang::move(const uint8_t& x,const uint8_t& y,const uint8_t& moment)
WAT34 6:0bee4b2bb400 67 {
WAT34 6:0bee4b2bb400 68 omni->computeXY((y-127)/127.0,(x-127)/127.0,moment);
WAT34 6:0bee4b2bb400 69 for (size_t i = 0; i < 4; i++) {
WAT34 6:0bee4b2bb400 70 omniMotor[i]->setSpeed((omni->getOutput(i)-127)/127.0);
WAT34 6:0bee4b2bb400 71 }
WAT34 1:35e874e10602 72 }
WAT34 1:35e874e10602 73
WAT34 1:35e874e10602 74 void boomerang::shootingEncoderCount()
WAT34 1:35e874e10602 75 {
WAT34 1:35e874e10602 76 motorRPM = (1.0/rpmCalc.read())*60;
WAT34 1:35e874e10602 77 rpmCalc.reset();
WAT34 1:35e874e10602 78 rpmCalc.start();
WAT34 1:35e874e10602 79 }
WAT34 1:35e874e10602 80
WAT34 1:35e874e10602 81 void boomerang::setTargetMotorSpeed(const int& targetRPM)
WAT34 1:35e874e10602 82 {
WAT34 2:aca690545ec9 83 speedPID->setSetPoint(targetRPM);
WAT34 1:35e874e10602 84 }
WAT34 1:35e874e10602 85
WAT34 12:9d21189a8eb0 86 void boomerang::setTargetArmAngle(const int& targetArmAngle)
WAT34 12:9d21189a8eb0 87 {
WAT34 12:9d21189a8eb0 88 armPID->setSetPoint(targetArmAngle);
WAT34 12:9d21189a8eb0 89 }
WAT34 1:35e874e10602 90
WAT34 2:aca690545ec9 91 void boomerang::update()
WAT34 1:35e874e10602 92 {
WAT34 12:9d21189a8eb0 93 armAngle = armEncoder->getPulses()/800.0*85.51;
WAT34 12:9d21189a8eb0 94 armPID->setProcessValue(armAngle);
WAT34 6:0bee4b2bb400 95 speedPID->setProcessValue(motorRPM);
WAT34 12:9d21189a8eb0 96 if(!calibrating)
WAT34 12:9d21189a8eb0 97 armMotor->setSpeed(armPID->compute());
WAT34 6:0bee4b2bb400 98 shootingMotor->setSpeed(speedPID->compute());
WAT34 4:c2b09fa31492 99 beltMotor->setSpeed(beltSpeed);
WAT34 1:35e874e10602 100 }
WAT34 1:35e874e10602 101
WAT34 2:aca690545ec9 102 bool boomerang::fire()
WAT34 1:35e874e10602 103 {
WAT34 4:c2b09fa31492 104 if(firing)
WAT34 4:c2b09fa31492 105 return false;
WAT34 4:c2b09fa31492 106 firing = true;
WAT34 4:c2b09fa31492 107 beltSpeed=0.4;
WAT34 6:0bee4b2bb400 108 beltTime.attach(callback(this,&boomerang::beltStop),1);
WAT34 2:aca690545ec9 109 spiral->rotate();
WAT34 4:c2b09fa31492 110 boomerang::update();
WAT34 2:aca690545ec9 111 return true;
WAT34 1:35e874e10602 112 }
WAT34 1:35e874e10602 113
WAT34 1:35e874e10602 114 void boomerang::beltStop()
WAT34 1:35e874e10602 115 {
WAT34 4:c2b09fa31492 116 beltSpeed = 0;
WAT34 4:c2b09fa31492 117 firing = false;
WAT34 4:c2b09fa31492 118 boomerang::update();
WAT34 4:c2b09fa31492 119
WAT34 1:35e874e10602 120 }
WAT34 1:35e874e10602 121
WAT34 12:9d21189a8eb0 122 int boomerang::calibrateArm()
WAT34 1:35e874e10602 123 {
WAT34 12:9d21189a8eb0 124 if(armLimit->read() == true)
WAT34 12:9d21189a8eb0 125 return 1;
WAT34 12:9d21189a8eb0 126
WAT34 12:9d21189a8eb0 127 calibrating = true;
WAT34 12:9d21189a8eb0 128 armLimit->enable_irq();
WAT34 12:9d21189a8eb0 129 armMotor->setSpeed(0.2);
WAT34 12:9d21189a8eb0 130 return 0;
WAT34 1:35e874e10602 131 }
WAT34 12:9d21189a8eb0 132
WAT34 12:9d21189a8eb0 133 void boomerang::endCalibration()
WAT34 12:9d21189a8eb0 134 {
WAT34 12:9d21189a8eb0 135 calibrating = false;
WAT34 12:9d21189a8eb0 136 armEncoder->reset();
WAT34 12:9d21189a8eb0 137 armLimit->disable_irq();
WAT34 12:9d21189a8eb0 138 armPID->setSetPoint(10);
WAT34 12:9d21189a8eb0 139 }