2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
hayatoBoomerang.cpp@25:fd4fb86b4148, 2017-12-01 (annotated)
- Committer:
- WAT34
- Date:
- Fri Dec 01 18:16:33 2017 +0900
- Revision:
- 25:fd4fb86b4148
- Parent:
- 18:4b629221c215
- Child:
- 20:d052a0679309
- Child:
- 21:aad4a4adc18c
hopee it workks
Who changed what in which revision?
User | Revision | Line number | New 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 | 6:0bee4b2bb400 | 104 | armAngle = armEncoder->getPulses()/800.0*85.51; |
WAT34 | 6:0bee4b2bb400 | 105 | armPID->setProcessValue(armAngle); |
WAT34 | 6:0bee4b2bb400 | 106 | speedPID->setProcessValue(motorRPM); |
WAT34 | 17:311aed3cad15 | 107 | armMotor->setSpeed(armPID->compute()); |
WAT34 | 6:0bee4b2bb400 | 108 | shootingMotor->setSpeed(speedPID->compute()); |
WAT34 | 17:311aed3cad15 | 109 | beltMotor->setSpeed(spiral->beltSpeed); |
WAT34 | 17:311aed3cad15 | 110 | spiralMotor->setSpeed(spiral->spiralSpeed); |
WAT34 | 1:35e874e10602 | 111 | } |
WAT34 | 1:35e874e10602 | 112 | |
WAT34 | 2:aca690545ec9 | 113 | bool boomerang::fire() |
WAT34 | 1:35e874e10602 | 114 | { |
WAT34 | 17:311aed3cad15 | 115 | if(spiral->firing) |
WAT34 | 4:c2b09fa31492 | 116 | return false; |
WAT34 | 2:aca690545ec9 | 117 | spiral->rotate(); |
WAT34 | 2:aca690545ec9 | 118 | return true; |
WAT34 | 1:35e874e10602 | 119 | } |