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?
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 | 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 | } |