Diff: AmmoSupplier.cpp

Revision:

0:49a747f09bd2

Child:

1:2a43c09cc3be

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AmmoSupplier.cpp	Fri Aug 21 04:52:23 2015 +0000
@@ -0,0 +1,35 @@
+#include "mbed.h"
+#include "AmmoSupplier.h"
+#include "I2CMotor.h"
+
+const float AmmoSupplier::mTimeout_sec              = 3.0f;
+const float AmmoSupplier::mDuty                     = 0.5f;
+const PinName AmmoSupplier::mLazerSensorDinPinName  = D5;
+
+AmmoSupplier::AmmoSupplier( I2CMotor** motor ){
+    mSupplier        = motor;
+    mAmmoLazerSensor = new DigitalIn( mLazerSensorDinPinName );
+    mTimer           = new Timer;
+    mIsSupplying     = false;
+}
+
+void AmmoSupplier::update(){
+    I2CMotor::ActionType action = I2CMotor::BRAKE;
+    
+    if ( mIsSupplying ){
+        if ( mTimer->read() > mTimeout_sec ){
+            mIsSupplying = false;
+            mTimer->stop();
+            mTimer->reset();
+        } else if ( mAmmoLazerSensor->read() ){
+            mIsSupplying = false;
+        } else {
+            action = I2CMotor::FORWARD;
+        }
+    }
+    
+    for ( int i = 0; i < 2; ++i ){
+        mSupplier[ i ]->setActionType( action );
+        mSupplier[ i ]->setDuty( mDuty );
+    }
+}