NagaokaRoboticsClub_mbedTeam / Mbed OS hayatoShooter

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

Dependencies:   PID QEI ikarashiMDC recieveController omni

hayatoBoomerang.cpp@11:68defd561031, 2017-09-01 (annotated)

Committer:
WAT34
Date:
Fri Sep 01 14:04:21 2017 +0900
Revision:
11:68defd561031
Parent:
6:0bee4b2bb400
Child:
12:9d21189a8eb0
damn

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 1:35e874e10602 7
WAT34 4:c2b09fa31492 8 //armLimit->mode(PullUp);
WAT34 2:aca690545ec9 9
WAT34 2:aca690545ec9 10 rscs = new DigitalOut(NC);
WAT34 1:35e874e10602 11
WAT34 1:35e874e10602 12 speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE);
WAT34 6:0bee4b2bb400 13 speedPID->setInputLimits(0.0,9000.0);
WAT34 2:aca690545ec9 14 speedPID->setOutputLimits(0,1.0);
WAT34 1:35e874e10602 15
WAT34 2:aca690545ec9 16
WAT34 2:aca690545ec9 17 speedPID->setBias(0);
WAT34 2:aca690545ec9 18 speedPID->setMode(AUTO_MODE);
WAT34 2:aca690545ec9 19 speedPID->setSetPoint(0);
WAT34 1:35e874e10602 20
WAT34 1:35e874e10602 21
WAT34 11:68defd561031 22 armValve[0] = new DigitalOut(D13);
WAT34 11:68defd561031 23 armValve[1] = new DigitalOut(D13);
WAT34 2:aca690545ec9 24 shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
WAT34 2:aca690545ec9 25 spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
WAT34 2:aca690545ec9 26 beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
WAT34 1:35e874e10602 27
WAT34 6:0bee4b2bb400 28 omni = new Omni(4);
WAT34 6:0bee4b2bb400 29 omniMotor[0] = new ikarashiMDC(rscs,1,0,SM,serial_);
WAT34 6:0bee4b2bb400 30 omniMotor[1] = new ikarashiMDC(rscs,1,1,SM,serial_);
WAT34 6:0bee4b2bb400 31 omniMotor[2] = new ikarashiMDC(rscs,1,2,SM,serial_);
WAT34 6:0bee4b2bb400 32 omniMotor[3] = new ikarashiMDC(rscs,1,3,SM,serial_);
WAT34 6:0bee4b2bb400 33 omniMotor[0]->braking=true;
WAT34 6:0bee4b2bb400 34 omniMotor[1]->braking=true;
WAT34 6:0bee4b2bb400 35 omniMotor[2]->braking=true;
WAT34 6:0bee4b2bb400 36 omniMotor[3]->braking=true;
WAT34 6:0bee4b2bb400 37
WAT34 11:68defd561031 38 armValve[0]->write(false);
WAT34 11:68defd561031 39 armValve[1]->write(false);
WAT34 4:c2b09fa31492 40 shootingMotor->setSpeed(0);
WAT34 4:c2b09fa31492 41 spiralMotor->setSpeed(0);
WAT34 4:c2b09fa31492 42 beltMotor->setSpeed(0);
WAT34 4:c2b09fa31492 43
WAT34 2:aca690545ec9 44 spiral = new Spiral(spiralMotor);
WAT34 1:35e874e10602 45
WAT34 1:35e874e10602 46 motorRPM = 0;
WAT34 1:35e874e10602 47
WAT34 11:68defd561031 48 omni->setWheelRadian(radians(45),radians(45+90),radians(45+180),radians(45+270));
WAT34 6:0bee4b2bb400 49
WAT34 2:aca690545ec9 50 speedEncoder->rise(this,&boomerang::shootingEncoderCount);
WAT34 2:aca690545ec9 51 speedEncoder->enable_irq();
WAT34 6:0bee4b2bb400 52 firing = false;
WAT34 6:0bee4b2bb400 53
WAT34 6:0bee4b2bb400 54 boomerang::update();
WAT34 6:0bee4b2bb400 55 }
WAT34 6:0bee4b2bb400 56
WAT34 6:0bee4b2bb400 57 void boomerang::move(const uint8_t& x,const uint8_t& y,const uint8_t& moment)
WAT34 6:0bee4b2bb400 58 {
WAT34 6:0bee4b2bb400 59 omni->computeXY((y-127)/127.0,(x-127)/127.0,moment);
WAT34 6:0bee4b2bb400 60 for (size_t i = 0; i < 4; i++) {
WAT34 6:0bee4b2bb400 61 omniMotor[i]->setSpeed((omni->getOutput(i)-127)/127.0);
WAT34 6:0bee4b2bb400 62 }
WAT34 1:35e874e10602 63 }
WAT34 1:35e874e10602 64
WAT34 1:35e874e10602 65 void boomerang::shootingEncoderCount()
WAT34 1:35e874e10602 66 {
WAT34 1:35e874e10602 67 motorRPM = (1.0/rpmCalc.read())*60;
WAT34 1:35e874e10602 68 rpmCalc.reset();
WAT34 1:35e874e10602 69 rpmCalc.start();
WAT34 1:35e874e10602 70 }
WAT34 1:35e874e10602 71
WAT34 1:35e874e10602 72 void boomerang::setTargetMotorSpeed(const int& targetRPM)
WAT34 1:35e874e10602 73 {
WAT34 2:aca690545ec9 74 speedPID->setSetPoint(targetRPM);
WAT34 1:35e874e10602 75 }
WAT34 1:35e874e10602 76
WAT34 1:35e874e10602 77
WAT34 2:aca690545ec9 78 void boomerang::update()
WAT34 1:35e874e10602 79 {
WAT34 6:0bee4b2bb400 80 speedPID->setProcessValue(motorRPM);
WAT34 6:0bee4b2bb400 81 shootingMotor->setSpeed(speedPID->compute());
WAT34 4:c2b09fa31492 82 beltMotor->setSpeed(beltSpeed);
WAT34 1:35e874e10602 83 }
WAT34 1:35e874e10602 84
WAT34 2:aca690545ec9 85 bool boomerang::fire()
WAT34 1:35e874e10602 86 {
WAT34 4:c2b09fa31492 87 if(firing)
WAT34 4:c2b09fa31492 88 return false;
WAT34 4:c2b09fa31492 89 firing = true;
WAT34 4:c2b09fa31492 90 beltSpeed=0.4;
WAT34 6:0bee4b2bb400 91 beltTime.attach(callback(this,&boomerang::beltStop),1);
WAT34 2:aca690545ec9 92 spiral->rotate();
WAT34 4:c2b09fa31492 93 boomerang::update();
WAT34 2:aca690545ec9 94 return true;
WAT34 1:35e874e10602 95 }
WAT34 1:35e874e10602 96
WAT34 1:35e874e10602 97 void boomerang::beltStop()
WAT34 1:35e874e10602 98 {
WAT34 4:c2b09fa31492 99 beltSpeed = 0;
WAT34 4:c2b09fa31492 100 firing = false;
WAT34 4:c2b09fa31492 101 boomerang::update();
WAT34 4:c2b09fa31492 102
WAT34 1:35e874e10602 103 }
WAT34 1:35e874e10602 104
WAT34 11:68defd561031 105 void boomerang::sword()
WAT34 1:35e874e10602 106 {
WAT34 11:68defd561031 107 armValve[0]->write(!armValve[0]->read());
WAT34 11:68defd561031 108 armValve[1]->write(!armValve[0]->read());
WAT34 1:35e874e10602 109 }