Diff: YMotorDriverBase.cpp

Revision:

0:a2bbf76ca734

Child:

1:c8ed08beefb9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/YMotorDriverBase.cpp	Sun Aug 23 15:18:04 2015 +0000
@@ -0,0 +1,126 @@
+#include "mbed.h"
+#include "PWMOut.h"
+#include "YMotorDriverBase.h"
+
+const PinName YMotorDriverBase::mMotorDriveDoutPinName[] = {
+    dp6, dp9, dp13, dp14
+};
+const PinName YMotorDriverBase::mMotorDrivePwmPinName   = dp1;
+const PinName YMotorDriverBase::mLEDPinName             = dp28;
+const PinName YMotorDriverBase::mSerialPinName[]        = {
+//  tx    rx
+    dp16, dp15
+};
+const PinName YMotorDriverBase::mI2CPinName[] = {
+//  sda  scl
+    dp5, dp27
+};
+const int YMotorDriverBase::mPwmCycle_us  = 300;
+const float YMotorDriverBase::mMaxDuty    = 0.90f;
+const float YMotorDriverBase::mMinDuty    = 0.30f;
+
+YMotorDriverBase::YMotorDriverBase( char address ) : mAddress( address ){
+    mI2C = new I2CSlave( mI2CPinName[ 0 ], mI2CPinName[ 1 ] );
+    mI2C->address( mAddress );
+    
+    for ( int i = 0; i < 4; ++i ){
+        mMotorDriveDout[ i ] = new DigitalOut( mMotorDriveDoutPinName[ i ] );
+    }
+    
+    mAction = RELEASE;
+    
+    mMotorDrivePwm = new PWMOut( mMotorDrivePwmPinName );
+    mMotorDrivePwm->setPeriod_us( mPwmCycle_us );
+    mMotorDrivePwm->setDuty( mMinDuty );
+    
+    mLED = new DigitalOut( mLEDPinName, 0 );
+    
+    write();
+}
+
+YMotorDriverBase::~YMotorDriverBase(){
+    delete mI2C;
+}
+
+void YMotorDriverBase::update(){
+     switch ( mI2C->receive() ){
+        case I2CSlave::ReadAddressed:{
+             char buf[] = { mMinDuty * 255.0f, mMaxDuty * 255.0f };
+             mI2C->write( buf, 2 );
+            break;
+        }
+        case I2CSlave::WriteGeneral:
+            break;
+             
+        case I2CSlave::WriteAddressed:{
+            char buf[ 2 ];
+            mI2C->read( buf, 2 );
+            // 初めの1Byteはモータの動作を指定する
+            mAction = static_cast< MotorAction >( buf[ 0 ] );
+            // 次の1Byteは0~255でDutyを指定する
+            mDuty = static_cast< float >( buf[ 1 ] ) / 255.0f;
+            break;
+        }
+            
+        case I2CSlave::NoData:
+            break;
+     }
+     
+     write();
+}
+void YMotorDriverBase::write(){
+    updatePwmDuty();
+    
+    switch ( mAction ){
+        case FORWARD:
+            mMotorDriveDout[ 0 ]->write( 0 );
+            mMotorDriveDout[ 1 ]->write( 1 );
+            mMotorDriveDout[ 2 ]->write( 1 );
+            mMotorDriveDout[ 3 ]->write( 0 );
+            break;
+            
+        case REVERSE:
+            mMotorDriveDout[ 0 ]->write( 1 );
+            mMotorDriveDout[ 1 ]->write( 0 );
+            mMotorDriveDout[ 2 ]->write( 0 );
+            mMotorDriveDout[ 3 ]->write( 1 );
+            break;
+            
+        case BRAKE:
+            mMotorDriveDout[ 0 ]->write( 0 );
+            mMotorDriveDout[ 1 ]->write( 0 );
+            mMotorDriveDout[ 2 ]->write( 0 );
+            mMotorDriveDout[ 3 ]->write( 0 );
+            break;
+            
+        case RELEASE:
+            mMotorDriveDout[ 0 ]->write( 1 );
+            mMotorDriveDout[ 1 ]->write( 1 );
+            mMotorDriveDout[ 2 ]->write( 0 );
+            mMotorDriveDout[ 3 ]->write( 0 );
+            break;
+            
+        default:
+            mAction = RELEASE;
+            mMotorDriveDout[ 0 ]->write( 1 );
+            mMotorDriveDout[ 1 ]->write( 1 );
+            mMotorDriveDout[ 2 ]->write( 0 );
+            mMotorDriveDout[ 3 ]->write( 0 );
+            break;
+    }
+}
+
+void YMotorDriverBase::updatePwmDuty(){
+    static float prevDuty = mMinDuty;
+    float d = middle( mMinDuty, mDuty, mMaxDuty );
+    
+    if ( d != prevDuty ){
+        mMotorDrivePwm->setDuty( d );
+    }
+    prevDuty = d;
+}
+
+void YMotorDriverBase::setPercent( float p ){
+    p = middle( 0.0f, p, 1.0f );
+    mDuty = p * mMaxDuty + ( 1.0f - p ) * mMinDuty;
+}