Diff: YMotorDriverSupplier.cpp

Revision:

0:bb84da068c45

Child:

1:9d8fe1f0ee36

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/YMotorDriverSupplier.cpp	Sun Aug 23 15:17:57 2015 +0000
@@ -0,0 +1,141 @@
+#include "YMotorDriverBase.h"
+#include "YMotorDriverSupplier.h"
+#include "mbed.h"
+
+const float YMotorDriverSupplier::mDuty = 0.30f;
+const PinName YMotorDriverSupplier::mDinPinName[] = {
+    dp2,    // lazer
+    dp4     // photointerrupter
+};
+const float YMotorDriverSupplier::mTimeout_sec = 2.5f;
+
+YMotorDriverSupplier::YMotorDriverSupplier( char address ) : YMotorDriverBase( address ){
+    mLazerDin           = new DigitalIn( mDinPinName[ LAZER ] );
+    mPhotointerrupter   = new InterruptIn( mDinPinName[ PHOTOINTERRUPTER ] );
+    mPhotointerrupter->fall( this, &YMotorDriverSupplier::itr );
+    mPhotointerrupter->rise( this, &YMotorDriverSupplier::itr );
+    mTimer = new Timer;
+    mTimer->stop();
+    mTimer->reset();
+    mLazerTimer = new Timer;
+    mIsWorking  = false;
+    mIsMayStoppable = false;
+    mHasFinished    = false;
+    mPrevInterrupterState = mPhotointerrupter->read();
+}
+
+void YMotorDriverSupplier::itr(){
+    /*
+    bool newState = mPhotointerrupter->read();
+    if ( newState != mPrevInterrupterState ){
+        mIsWorking = false;
+        mPrevInterrupterState = newState;
+    }
+    */
+    float validSignalWidth_us = 80  * 1000;
+    //
+    if ( mTimer->read_us() > validSignalWidth_us ){
+        //
+        //mIsWorking = false;
+        
+        mHasFinished = true;
+        mTimer->reset();
+        mTimer->stop();
+        mLazerTimer->reset();
+        mLazerTimer->start();
+        //
+    }
+    //
+    
+    /*
+    mIsStoppable = true;
+    mNewInterrupterState = mPhotointerrupter->read();
+    mTimer->start();
+    */
+}
+
+void YMotorDriverSupplier::update(){
+    switch ( mI2C->receive() ){
+        case I2CSlave::ReadAddressed:{
+            char buf = mIsWorking;
+            mI2C->write( &buf, 1 );
+            break;
+        }
+        case I2CSlave::WriteGeneral:
+            break;
+        
+        case I2CSlave::WriteAddressed:{
+            char buf;
+            mI2C->read( &buf, 1 );
+            
+            //
+            if ( buf && !mIsWorking ){
+                mTimer->reset();
+                mTimer->start();
+            }
+            //
+            
+            mIsWorking = buf;
+            break;
+        }
+        
+        case I2CSlave::NoData:
+            break;
+    }
+    
+    updateSupplier();
+    write();
+}
+
+void YMotorDriverSupplier::updateSupplier(){
+    /*
+    bool hasFinished = mIsWorking;
+    hasFinished &= !mLazerDin->read();
+    hasFinished |= ( mTimer->read() > mTimeout_sec );
+    if ( hasFinished ){
+        mIsWorking = false;
+        mTimer->stop();
+        mTimer->reset();
+    }
+    */
+    /*
+    float validSignalWidth_us = 100;
+    if ( mIsMayStoppable ){
+        if ( mTimer->read_us() > validSignalWidth_us ){
+            mIsMayStoppable = false;
+            mIsWorking = false;
+            mTimer->stop();
+            mTimer->reset();
+        }
+    }
+    */
+    
+    MotorAction action = BRAKE;
+    
+    int fallingAmmoTime_us = 500 * 1000;
+    if ( mHasFinished ){
+        if ( !mLazerDin->read() ){
+            mIsWorking = false;
+            mHasFinished = false;
+            mLazerTimer->stop();
+            mLazerTimer->reset();
+        } else if ( mLazerTimer->read_us() > fallingAmmoTime_us ){
+            mHasFinished = false;
+            mLazerTimer->stop();
+            mLazerTimer->reset();
+            mTimer->reset();
+            mTimer->start();
+        }
+    } else if ( mIsWorking ){
+        action = FORWARD;
+        setDuty( mDuty );
+    }
+    
+    /*
+    if ( mIsWorking ){
+        action = FORWARD;
+        setDuty( mDuty );
+    }
+    */
+    setMotorAction( action );
+}