tarou yamada / YMotorDriverBase

Dependents:   YMotor

YMotorDriverBase.cpp@0:a2bbf76ca734, 2015-08-23 (annotated)

Committer:
inst
Date:
Sun Aug 23 15:18:04 2015 +0000
Revision:
0:a2bbf76ca734
Child:
1:c8ed08beefb9
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
inst 0:a2bbf76ca734 1 #include "mbed.h"
inst 0:a2bbf76ca734 2 #include "PWMOut.h"
inst 0:a2bbf76ca734 3 #include "YMotorDriverBase.h"
inst 0:a2bbf76ca734 4
inst 0:a2bbf76ca734 5 const PinName YMotorDriverBase::mMotorDriveDoutPinName[] = {
inst 0:a2bbf76ca734 6 dp6, dp9, dp13, dp14
inst 0:a2bbf76ca734 7 };
inst 0:a2bbf76ca734 8 const PinName YMotorDriverBase::mMotorDrivePwmPinName = dp1;
inst 0:a2bbf76ca734 9 const PinName YMotorDriverBase::mLEDPinName = dp28;
inst 0:a2bbf76ca734 10 const PinName YMotorDriverBase::mSerialPinName[] = {
inst 0:a2bbf76ca734 11 // tx rx
inst 0:a2bbf76ca734 12 dp16, dp15
inst 0:a2bbf76ca734 13 };
inst 0:a2bbf76ca734 14 const PinName YMotorDriverBase::mI2CPinName[] = {
inst 0:a2bbf76ca734 15 // sda scl
inst 0:a2bbf76ca734 16 dp5, dp27
inst 0:a2bbf76ca734 17 };
inst 0:a2bbf76ca734 18 const int YMotorDriverBase::mPwmCycle_us = 300;
inst 0:a2bbf76ca734 19 const float YMotorDriverBase::mMaxDuty = 0.90f;
inst 0:a2bbf76ca734 20 const float YMotorDriverBase::mMinDuty = 0.30f;
inst 0:a2bbf76ca734 21
inst 0:a2bbf76ca734 22 YMotorDriverBase::YMotorDriverBase( char address ) : mAddress( address ){
inst 0:a2bbf76ca734 23 mI2C = new I2CSlave( mI2CPinName[ 0 ], mI2CPinName[ 1 ] );
inst 0:a2bbf76ca734 24 mI2C->address( mAddress );
inst 0:a2bbf76ca734 25
inst 0:a2bbf76ca734 26 for ( int i = 0; i < 4; ++i ){
inst 0:a2bbf76ca734 27 mMotorDriveDout[ i ] = new DigitalOut( mMotorDriveDoutPinName[ i ] );
inst 0:a2bbf76ca734 28 }
inst 0:a2bbf76ca734 29
inst 0:a2bbf76ca734 30 mAction = RELEASE;
inst 0:a2bbf76ca734 31
inst 0:a2bbf76ca734 32 mMotorDrivePwm = new PWMOut( mMotorDrivePwmPinName );
inst 0:a2bbf76ca734 33 mMotorDrivePwm->setPeriod_us( mPwmCycle_us );
inst 0:a2bbf76ca734 34 mMotorDrivePwm->setDuty( mMinDuty );
inst 0:a2bbf76ca734 35
inst 0:a2bbf76ca734 36 mLED = new DigitalOut( mLEDPinName, 0 );
inst 0:a2bbf76ca734 37
inst 0:a2bbf76ca734 38 write();
inst 0:a2bbf76ca734 39 }
inst 0:a2bbf76ca734 40
inst 0:a2bbf76ca734 41 YMotorDriverBase::~YMotorDriverBase(){
inst 0:a2bbf76ca734 42 delete mI2C;
inst 0:a2bbf76ca734 43 }
inst 0:a2bbf76ca734 44
inst 0:a2bbf76ca734 45 void YMotorDriverBase::update(){
inst 0:a2bbf76ca734 46 switch ( mI2C->receive() ){
inst 0:a2bbf76ca734 47 case I2CSlave::ReadAddressed:{
inst 0:a2bbf76ca734 48 char buf[] = { mMinDuty * 255.0f, mMaxDuty * 255.0f };
inst 0:a2bbf76ca734 49 mI2C->write( buf, 2 );
inst 0:a2bbf76ca734 50 break;
inst 0:a2bbf76ca734 51 }
inst 0:a2bbf76ca734 52 case I2CSlave::WriteGeneral:
inst 0:a2bbf76ca734 53 break;
inst 0:a2bbf76ca734 54
inst 0:a2bbf76ca734 55 case I2CSlave::WriteAddressed:{
inst 0:a2bbf76ca734 56 char buf[ 2 ];
inst 0:a2bbf76ca734 57 mI2C->read( buf, 2 );
inst 0:a2bbf76ca734 58 // 初めの1Byteはモータの動作を指定する
inst 0:a2bbf76ca734 59 mAction = static_cast< MotorAction >( buf[ 0 ] );
inst 0:a2bbf76ca734 60 // 次の1Byteは0~255でDutyを指定する
inst 0:a2bbf76ca734 61 mDuty = static_cast< float >( buf[ 1 ] ) / 255.0f;
inst 0:a2bbf76ca734 62 break;
inst 0:a2bbf76ca734 63 }
inst 0:a2bbf76ca734 64
inst 0:a2bbf76ca734 65 case I2CSlave::NoData:
inst 0:a2bbf76ca734 66 break;
inst 0:a2bbf76ca734 67 }
inst 0:a2bbf76ca734 68
inst 0:a2bbf76ca734 69 write();
inst 0:a2bbf76ca734 70 }
inst 0:a2bbf76ca734 71 void YMotorDriverBase::write(){
inst 0:a2bbf76ca734 72 updatePwmDuty();
inst 0:a2bbf76ca734 73
inst 0:a2bbf76ca734 74 switch ( mAction ){
inst 0:a2bbf76ca734 75 case FORWARD:
inst 0:a2bbf76ca734 76 mMotorDriveDout[ 0 ]->write( 0 );
inst 0:a2bbf76ca734 77 mMotorDriveDout[ 1 ]->write( 1 );
inst 0:a2bbf76ca734 78 mMotorDriveDout[ 2 ]->write( 1 );
inst 0:a2bbf76ca734 79 mMotorDriveDout[ 3 ]->write( 0 );
inst 0:a2bbf76ca734 80 break;
inst 0:a2bbf76ca734 81
inst 0:a2bbf76ca734 82 case REVERSE:
inst 0:a2bbf76ca734 83 mMotorDriveDout[ 0 ]->write( 1 );
inst 0:a2bbf76ca734 84 mMotorDriveDout[ 1 ]->write( 0 );
inst 0:a2bbf76ca734 85 mMotorDriveDout[ 2 ]->write( 0 );
inst 0:a2bbf76ca734 86 mMotorDriveDout[ 3 ]->write( 1 );
inst 0:a2bbf76ca734 87 break;
inst 0:a2bbf76ca734 88
inst 0:a2bbf76ca734 89 case BRAKE:
inst 0:a2bbf76ca734 90 mMotorDriveDout[ 0 ]->write( 0 );
inst 0:a2bbf76ca734 91 mMotorDriveDout[ 1 ]->write( 0 );
inst 0:a2bbf76ca734 92 mMotorDriveDout[ 2 ]->write( 0 );
inst 0:a2bbf76ca734 93 mMotorDriveDout[ 3 ]->write( 0 );
inst 0:a2bbf76ca734 94 break;
inst 0:a2bbf76ca734 95
inst 0:a2bbf76ca734 96 case RELEASE:
inst 0:a2bbf76ca734 97 mMotorDriveDout[ 0 ]->write( 1 );
inst 0:a2bbf76ca734 98 mMotorDriveDout[ 1 ]->write( 1 );
inst 0:a2bbf76ca734 99 mMotorDriveDout[ 2 ]->write( 0 );
inst 0:a2bbf76ca734 100 mMotorDriveDout[ 3 ]->write( 0 );
inst 0:a2bbf76ca734 101 break;
inst 0:a2bbf76ca734 102
inst 0:a2bbf76ca734 103 default:
inst 0:a2bbf76ca734 104 mAction = RELEASE;
inst 0:a2bbf76ca734 105 mMotorDriveDout[ 0 ]->write( 1 );
inst 0:a2bbf76ca734 106 mMotorDriveDout[ 1 ]->write( 1 );
inst 0:a2bbf76ca734 107 mMotorDriveDout[ 2 ]->write( 0 );
inst 0:a2bbf76ca734 108 mMotorDriveDout[ 3 ]->write( 0 );
inst 0:a2bbf76ca734 109 break;
inst 0:a2bbf76ca734 110 }
inst 0:a2bbf76ca734 111 }
inst 0:a2bbf76ca734 112
inst 0:a2bbf76ca734 113 void YMotorDriverBase::updatePwmDuty(){
inst 0:a2bbf76ca734 114 static float prevDuty = mMinDuty;
inst 0:a2bbf76ca734 115 float d = middle( mMinDuty, mDuty, mMaxDuty );
inst 0:a2bbf76ca734 116
inst 0:a2bbf76ca734 117 if ( d != prevDuty ){
inst 0:a2bbf76ca734 118 mMotorDrivePwm->setDuty( d );
inst 0:a2bbf76ca734 119 }
inst 0:a2bbf76ca734 120 prevDuty = d;
inst 0:a2bbf76ca734 121 }
inst 0:a2bbf76ca734 122
inst 0:a2bbf76ca734 123 void YMotorDriverBase::setPercent( float p ){
inst 0:a2bbf76ca734 124 p = middle( 0.0f, p, 1.0f );
inst 0:a2bbf76ca734 125 mDuty = p * mMaxDuty + ( 1.0f - p ) * mMinDuty;
inst 0:a2bbf76ca734 126 }