YMotorDriverBase.cpp
- Committer:
- inst
- Date:
- 2015-11-13
- Revision:
- 2:871d1f6d311e
- Parent:
- 1:c8ed08beefb9
- Child:
- 3:22f19e076931
File content as of revision 2:871d1f6d311e:
#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; }