tarou yamada / YMotorDriverSupplier

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?

UserRevisionLine numberNew 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 }