File content as of revision 3:22f19e076931:

#include "mbed.h"
#include "PWMOut.h"
#include "YMotorDriverBase.h"
#include "YMotorDriver.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::mMinDutyList[] = {
    0.285f, // steering
    0.285f, //
    0.285f, //
    0.0f,   // shooter
    0.3f,   // general
    0.22f   // supplier
};
const float YMotorDriverBase::mMaxDutyList[] = {
    0.85f,  // steering
    0.85f,  // 
    0.85f,  // 
    1.0f,   // shooter
    0.9f,   // general
    0.50f   // supplier
};

YMotorDriverBase::YMotorDriverBase( char address ) :
        mAddress( address ),
        mMaxDuty( mMaxDutyList[ YMotorDriver::GENERAL_ID ] ), mMinDuty( mMinDutyList[ YMotorDriver::GENERAL_ID ] ){
    init();
}

YMotorDriverBase::YMotorDriverBase( char address, int id ) :
        mAddress( address ), mMaxDuty( mMaxDutyList[ id ] ), mMinDuty( mMinDutyList[ id ] ){
    init();
}

YMotorDriverBase::YMotorDriverBase( char address, float maxDuty, float minDuty ) :
        mAddress( address ), mMaxDuty( maxDuty ), mMinDuty( minDuty ){
    init();
}

void YMotorDriverBase::init(){
    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(){
     updateI2CSlave();
     updateSpecial();
     write();
}

void YMotorDriverBase::updateI2CSlave(){
     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;
     }
}

void YMotorDriverBase::write(){
    mMotorDrivePwm->setDuty( middle( mMinDuty, mDuty, mMaxDuty ) );
    
    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::setPercent( float p ){
    p = middle( 0.0f, p, 1.0f );
    mDuty = p * mMaxDuty + ( 1.0f - p ) * mMinDuty;
}