YMotorDriverSupplier.cpp@0:bb84da068c45, 2015-08-23 (annotated)
- Committer:
- inst
- Date:
- Sun Aug 23 15:17:57 2015 +0000
- Revision:
- 0:bb84da068c45
- Child:
- 1:9d8fe1f0ee36
first
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
inst | 0:bb84da068c45 | 1 | #include "YMotorDriverBase.h" |
inst | 0:bb84da068c45 | 2 | #include "YMotorDriverSupplier.h" |
inst | 0:bb84da068c45 | 3 | #include "mbed.h" |
inst | 0:bb84da068c45 | 4 | |
inst | 0:bb84da068c45 | 5 | const float YMotorDriverSupplier::mDuty = 0.30f; |
inst | 0:bb84da068c45 | 6 | const PinName YMotorDriverSupplier::mDinPinName[] = { |
inst | 0:bb84da068c45 | 7 | dp2, // lazer |
inst | 0:bb84da068c45 | 8 | dp4 // photointerrupter |
inst | 0:bb84da068c45 | 9 | }; |
inst | 0:bb84da068c45 | 10 | const float YMotorDriverSupplier::mTimeout_sec = 2.5f; |
inst | 0:bb84da068c45 | 11 | |
inst | 0:bb84da068c45 | 12 | YMotorDriverSupplier::YMotorDriverSupplier( char address ) : YMotorDriverBase( address ){ |
inst | 0:bb84da068c45 | 13 | mLazerDin = new DigitalIn( mDinPinName[ LAZER ] ); |
inst | 0:bb84da068c45 | 14 | mPhotointerrupter = new InterruptIn( mDinPinName[ PHOTOINTERRUPTER ] ); |
inst | 0:bb84da068c45 | 15 | mPhotointerrupter->fall( this, &YMotorDriverSupplier::itr ); |
inst | 0:bb84da068c45 | 16 | mPhotointerrupter->rise( this, &YMotorDriverSupplier::itr ); |
inst | 0:bb84da068c45 | 17 | mTimer = new Timer; |
inst | 0:bb84da068c45 | 18 | mTimer->stop(); |
inst | 0:bb84da068c45 | 19 | mTimer->reset(); |
inst | 0:bb84da068c45 | 20 | mLazerTimer = new Timer; |
inst | 0:bb84da068c45 | 21 | mIsWorking = false; |
inst | 0:bb84da068c45 | 22 | mIsMayStoppable = false; |
inst | 0:bb84da068c45 | 23 | mHasFinished = false; |
inst | 0:bb84da068c45 | 24 | mPrevInterrupterState = mPhotointerrupter->read(); |
inst | 0:bb84da068c45 | 25 | } |
inst | 0:bb84da068c45 | 26 | |
inst | 0:bb84da068c45 | 27 | void YMotorDriverSupplier::itr(){ |
inst | 0:bb84da068c45 | 28 | /* |
inst | 0:bb84da068c45 | 29 | bool newState = mPhotointerrupter->read(); |
inst | 0:bb84da068c45 | 30 | if ( newState != mPrevInterrupterState ){ |
inst | 0:bb84da068c45 | 31 | mIsWorking = false; |
inst | 0:bb84da068c45 | 32 | mPrevInterrupterState = newState; |
inst | 0:bb84da068c45 | 33 | } |
inst | 0:bb84da068c45 | 34 | */ |
inst | 0:bb84da068c45 | 35 | float validSignalWidth_us = 80 * 1000; |
inst | 0:bb84da068c45 | 36 | // |
inst | 0:bb84da068c45 | 37 | if ( mTimer->read_us() > validSignalWidth_us ){ |
inst | 0:bb84da068c45 | 38 | // |
inst | 0:bb84da068c45 | 39 | //mIsWorking = false; |
inst | 0:bb84da068c45 | 40 | |
inst | 0:bb84da068c45 | 41 | mHasFinished = true; |
inst | 0:bb84da068c45 | 42 | mTimer->reset(); |
inst | 0:bb84da068c45 | 43 | mTimer->stop(); |
inst | 0:bb84da068c45 | 44 | mLazerTimer->reset(); |
inst | 0:bb84da068c45 | 45 | mLazerTimer->start(); |
inst | 0:bb84da068c45 | 46 | // |
inst | 0:bb84da068c45 | 47 | } |
inst | 0:bb84da068c45 | 48 | // |
inst | 0:bb84da068c45 | 49 | |
inst | 0:bb84da068c45 | 50 | /* |
inst | 0:bb84da068c45 | 51 | mIsStoppable = true; |
inst | 0:bb84da068c45 | 52 | mNewInterrupterState = mPhotointerrupter->read(); |
inst | 0:bb84da068c45 | 53 | mTimer->start(); |
inst | 0:bb84da068c45 | 54 | */ |
inst | 0:bb84da068c45 | 55 | } |
inst | 0:bb84da068c45 | 56 | |
inst | 0:bb84da068c45 | 57 | void YMotorDriverSupplier::update(){ |
inst | 0:bb84da068c45 | 58 | switch ( mI2C->receive() ){ |
inst | 0:bb84da068c45 | 59 | case I2CSlave::ReadAddressed:{ |
inst | 0:bb84da068c45 | 60 | char buf = mIsWorking; |
inst | 0:bb84da068c45 | 61 | mI2C->write( &buf, 1 ); |
inst | 0:bb84da068c45 | 62 | break; |
inst | 0:bb84da068c45 | 63 | } |
inst | 0:bb84da068c45 | 64 | case I2CSlave::WriteGeneral: |
inst | 0:bb84da068c45 | 65 | break; |
inst | 0:bb84da068c45 | 66 | |
inst | 0:bb84da068c45 | 67 | case I2CSlave::WriteAddressed:{ |
inst | 0:bb84da068c45 | 68 | char buf; |
inst | 0:bb84da068c45 | 69 | mI2C->read( &buf, 1 ); |
inst | 0:bb84da068c45 | 70 | |
inst | 0:bb84da068c45 | 71 | // |
inst | 0:bb84da068c45 | 72 | if ( buf && !mIsWorking ){ |
inst | 0:bb84da068c45 | 73 | mTimer->reset(); |
inst | 0:bb84da068c45 | 74 | mTimer->start(); |
inst | 0:bb84da068c45 | 75 | } |
inst | 0:bb84da068c45 | 76 | // |
inst | 0:bb84da068c45 | 77 | |
inst | 0:bb84da068c45 | 78 | mIsWorking = buf; |
inst | 0:bb84da068c45 | 79 | break; |
inst | 0:bb84da068c45 | 80 | } |
inst | 0:bb84da068c45 | 81 | |
inst | 0:bb84da068c45 | 82 | case I2CSlave::NoData: |
inst | 0:bb84da068c45 | 83 | break; |
inst | 0:bb84da068c45 | 84 | } |
inst | 0:bb84da068c45 | 85 | |
inst | 0:bb84da068c45 | 86 | updateSupplier(); |
inst | 0:bb84da068c45 | 87 | write(); |
inst | 0:bb84da068c45 | 88 | } |
inst | 0:bb84da068c45 | 89 | |
inst | 0:bb84da068c45 | 90 | void YMotorDriverSupplier::updateSupplier(){ |
inst | 0:bb84da068c45 | 91 | /* |
inst | 0:bb84da068c45 | 92 | bool hasFinished = mIsWorking; |
inst | 0:bb84da068c45 | 93 | hasFinished &= !mLazerDin->read(); |
inst | 0:bb84da068c45 | 94 | hasFinished |= ( mTimer->read() > mTimeout_sec ); |
inst | 0:bb84da068c45 | 95 | if ( hasFinished ){ |
inst | 0:bb84da068c45 | 96 | mIsWorking = false; |
inst | 0:bb84da068c45 | 97 | mTimer->stop(); |
inst | 0:bb84da068c45 | 98 | mTimer->reset(); |
inst | 0:bb84da068c45 | 99 | } |
inst | 0:bb84da068c45 | 100 | */ |
inst | 0:bb84da068c45 | 101 | /* |
inst | 0:bb84da068c45 | 102 | float validSignalWidth_us = 100; |
inst | 0:bb84da068c45 | 103 | if ( mIsMayStoppable ){ |
inst | 0:bb84da068c45 | 104 | if ( mTimer->read_us() > validSignalWidth_us ){ |
inst | 0:bb84da068c45 | 105 | mIsMayStoppable = false; |
inst | 0:bb84da068c45 | 106 | mIsWorking = false; |
inst | 0:bb84da068c45 | 107 | mTimer->stop(); |
inst | 0:bb84da068c45 | 108 | mTimer->reset(); |
inst | 0:bb84da068c45 | 109 | } |
inst | 0:bb84da068c45 | 110 | } |
inst | 0:bb84da068c45 | 111 | */ |
inst | 0:bb84da068c45 | 112 | |
inst | 0:bb84da068c45 | 113 | MotorAction action = BRAKE; |
inst | 0:bb84da068c45 | 114 | |
inst | 0:bb84da068c45 | 115 | int fallingAmmoTime_us = 500 * 1000; |
inst | 0:bb84da068c45 | 116 | if ( mHasFinished ){ |
inst | 0:bb84da068c45 | 117 | if ( !mLazerDin->read() ){ |
inst | 0:bb84da068c45 | 118 | mIsWorking = false; |
inst | 0:bb84da068c45 | 119 | mHasFinished = false; |
inst | 0:bb84da068c45 | 120 | mLazerTimer->stop(); |
inst | 0:bb84da068c45 | 121 | mLazerTimer->reset(); |
inst | 0:bb84da068c45 | 122 | } else if ( mLazerTimer->read_us() > fallingAmmoTime_us ){ |
inst | 0:bb84da068c45 | 123 | mHasFinished = false; |
inst | 0:bb84da068c45 | 124 | mLazerTimer->stop(); |
inst | 0:bb84da068c45 | 125 | mLazerTimer->reset(); |
inst | 0:bb84da068c45 | 126 | mTimer->reset(); |
inst | 0:bb84da068c45 | 127 | mTimer->start(); |
inst | 0:bb84da068c45 | 128 | } |
inst | 0:bb84da068c45 | 129 | } else if ( mIsWorking ){ |
inst | 0:bb84da068c45 | 130 | action = FORWARD; |
inst | 0:bb84da068c45 | 131 | setDuty( mDuty ); |
inst | 0:bb84da068c45 | 132 | } |
inst | 0:bb84da068c45 | 133 | |
inst | 0:bb84da068c45 | 134 | /* |
inst | 0:bb84da068c45 | 135 | if ( mIsWorking ){ |
inst | 0:bb84da068c45 | 136 | action = FORWARD; |
inst | 0:bb84da068c45 | 137 | setDuty( mDuty ); |
inst | 0:bb84da068c45 | 138 | } |
inst | 0:bb84da068c45 | 139 | */ |
inst | 0:bb84da068c45 | 140 | setMotorAction( action ); |
inst | 0:bb84da068c45 | 141 | } |