tarou yamada / YMotorDriverBase

Dependents:   YMotor

YMotorDriverBase.cpp@2:871d1f6d311e, 2015-11-13 (annotated)

Committer:
inst
Date:
Fri Nov 13 07:47:35 2015 +0000
Revision:
2:871d1f6d311e
Parent:
1:c8ed08beefb9
Child:
3:22f19e076931
b

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 1:c8ed08beefb9 4 #include "YMotorDriver.h"
inst 0:a2bbf76ca734 5
inst 0:a2bbf76ca734 6 const PinName YMotorDriverBase::mMotorDriveDoutPinName[] = {
inst 0:a2bbf76ca734 7 dp6, dp9, dp13, dp14
inst 0:a2bbf76ca734 8 };
inst 0:a2bbf76ca734 9 const PinName YMotorDriverBase::mMotorDrivePwmPinName = dp1;
inst 0:a2bbf76ca734 10 const PinName YMotorDriverBase::mLEDPinName = dp28;
inst 0:a2bbf76ca734 11 const PinName YMotorDriverBase::mSerialPinName[] = {
inst 0:a2bbf76ca734 12 // tx rx
inst 0:a2bbf76ca734 13 dp16, dp15
inst 0:a2bbf76ca734 14 };
inst 0:a2bbf76ca734 15 const PinName YMotorDriverBase::mI2CPinName[] = {
inst 0:a2bbf76ca734 16 // sda scl
inst 0:a2bbf76ca734 17 dp5, dp27
inst 0:a2bbf76ca734 18 };
inst 0:a2bbf76ca734 19 const int YMotorDriverBase::mPwmCycle_us = 300;
inst 1:c8ed08beefb9 20 const float YMotorDriverBase::mMinDutyList[] = {
inst 1:c8ed08beefb9 21 0.285f, // steering
inst 1:c8ed08beefb9 22 0.285f, //
inst 1:c8ed08beefb9 23 0.285f, //
inst 1:c8ed08beefb9 24 0.0f, // shooter
inst 1:c8ed08beefb9 25 0.3f, // general
inst 1:c8ed08beefb9 26 0.22f // supplier
inst 1:c8ed08beefb9 27 };
inst 1:c8ed08beefb9 28 const float YMotorDriverBase::mMaxDutyList[] = {
inst 1:c8ed08beefb9 29 0.85f, // steering
inst 1:c8ed08beefb9 30 0.85f, //
inst 1:c8ed08beefb9 31 0.85f, //
inst 1:c8ed08beefb9 32 1.0f, // shooter
inst 1:c8ed08beefb9 33 0.9f, // general
inst 1:c8ed08beefb9 34 0.50f // supplier
inst 1:c8ed08beefb9 35 };
inst 0:a2bbf76ca734 36
inst 1:c8ed08beefb9 37 YMotorDriverBase::YMotorDriverBase( char address ) :
inst 1:c8ed08beefb9 38 mAddress( address ),
inst 1:c8ed08beefb9 39 mMaxDuty( mMaxDutyList[ YMotorDriver::GENERAL_ID ] ), mMinDuty( mMinDutyList[ YMotorDriver::GENERAL_ID ] ){
inst 1:c8ed08beefb9 40
inst 1:c8ed08beefb9 41 init();
inst 1:c8ed08beefb9 42 }
inst 1:c8ed08beefb9 43
inst 1:c8ed08beefb9 44 YMotorDriverBase::YMotorDriverBase( char address, int id ) :
inst 1:c8ed08beefb9 45 mAddress( address ), mMaxDuty( mMaxDutyList[ id ] ), mMinDuty( mMinDutyList[ id ] ){
inst 1:c8ed08beefb9 46 init();
inst 1:c8ed08beefb9 47 }
inst 1:c8ed08beefb9 48
inst 1:c8ed08beefb9 49 YMotorDriverBase::YMotorDriverBase( char address, float maxDuty, float minDuty ) :
inst 1:c8ed08beefb9 50 mAddress( address ), mMaxDuty( maxDuty ), mMinDuty( minDuty ){
inst 1:c8ed08beefb9 51 init();
inst 1:c8ed08beefb9 52 }
inst 1:c8ed08beefb9 53
inst 1:c8ed08beefb9 54 void YMotorDriverBase::init(){
inst 0:a2bbf76ca734 55 mI2C = new I2CSlave( mI2CPinName[ 0 ], mI2CPinName[ 1 ] );
inst 0:a2bbf76ca734 56 mI2C->address( mAddress );
inst 0:a2bbf76ca734 57
inst 0:a2bbf76ca734 58 for ( int i = 0; i < 4; ++i ){
inst 0:a2bbf76ca734 59 mMotorDriveDout[ i ] = new DigitalOut( mMotorDriveDoutPinName[ i ] );
inst 0:a2bbf76ca734 60 }
inst 0:a2bbf76ca734 61
inst 0:a2bbf76ca734 62 mAction = RELEASE;
inst 0:a2bbf76ca734 63
inst 0:a2bbf76ca734 64 mMotorDrivePwm = new PWMOut( mMotorDrivePwmPinName );
inst 0:a2bbf76ca734 65 mMotorDrivePwm->setPeriod_us( mPwmCycle_us );
inst 0:a2bbf76ca734 66 mMotorDrivePwm->setDuty( mMinDuty );
inst 0:a2bbf76ca734 67
inst 0:a2bbf76ca734 68 mLED = new DigitalOut( mLEDPinName, 0 );
inst 0:a2bbf76ca734 69
inst 0:a2bbf76ca734 70 write();
inst 0:a2bbf76ca734 71 }
inst 0:a2bbf76ca734 72
inst 0:a2bbf76ca734 73 YMotorDriverBase::~YMotorDriverBase(){
inst 0:a2bbf76ca734 74 delete mI2C;
inst 0:a2bbf76ca734 75 }
inst 0:a2bbf76ca734 76
inst 0:a2bbf76ca734 77 void YMotorDriverBase::update(){
inst 1:c8ed08beefb9 78 updateI2CSlave();
inst 1:c8ed08beefb9 79 updateSpecial();
inst 1:c8ed08beefb9 80 write();
inst 1:c8ed08beefb9 81 }
inst 1:c8ed08beefb9 82
inst 1:c8ed08beefb9 83 void YMotorDriverBase::updateI2CSlave(){
inst 0:a2bbf76ca734 84 switch ( mI2C->receive() ){
inst 0:a2bbf76ca734 85 case I2CSlave::ReadAddressed:{
inst 0:a2bbf76ca734 86 char buf[] = { mMinDuty * 255.0f, mMaxDuty * 255.0f };
inst 0:a2bbf76ca734 87 mI2C->write( buf, 2 );
inst 0:a2bbf76ca734 88 break;
inst 0:a2bbf76ca734 89 }
inst 0:a2bbf76ca734 90 case I2CSlave::WriteGeneral:
inst 0:a2bbf76ca734 91 break;
inst 0:a2bbf76ca734 92
inst 0:a2bbf76ca734 93 case I2CSlave::WriteAddressed:{
inst 0:a2bbf76ca734 94 char buf[ 2 ];
inst 0:a2bbf76ca734 95 mI2C->read( buf, 2 );
inst 0:a2bbf76ca734 96 // 初めの1Byteはモータの動作を指定する
inst 0:a2bbf76ca734 97 mAction = static_cast< MotorAction >( buf[ 0 ] );
inst 0:a2bbf76ca734 98 // 次の1Byteは0~255でDutyを指定する
inst 0:a2bbf76ca734 99 mDuty = static_cast< float >( buf[ 1 ] ) / 255.0f;
inst 0:a2bbf76ca734 100 break;
inst 0:a2bbf76ca734 101 }
inst 0:a2bbf76ca734 102
inst 0:a2bbf76ca734 103 case I2CSlave::NoData:
inst 0:a2bbf76ca734 104 break;
inst 0:a2bbf76ca734 105 }
inst 0:a2bbf76ca734 106 }
inst 1:c8ed08beefb9 107
inst 0:a2bbf76ca734 108 void YMotorDriverBase::write(){
inst 2:871d1f6d311e 109 mMotorDrivePwm->setDuty( middle( mMinDuty, mDuty, mMaxDuty ) );
inst 0:a2bbf76ca734 110
inst 0:a2bbf76ca734 111 switch ( mAction ){
inst 0:a2bbf76ca734 112 case FORWARD:
inst 0:a2bbf76ca734 113 mMotorDriveDout[ 0 ]->write( 0 );
inst 0:a2bbf76ca734 114 mMotorDriveDout[ 1 ]->write( 1 );
inst 0:a2bbf76ca734 115 mMotorDriveDout[ 2 ]->write( 1 );
inst 0:a2bbf76ca734 116 mMotorDriveDout[ 3 ]->write( 0 );
inst 0:a2bbf76ca734 117 break;
inst 0:a2bbf76ca734 118
inst 0:a2bbf76ca734 119 case REVERSE:
inst 0:a2bbf76ca734 120 mMotorDriveDout[ 0 ]->write( 1 );
inst 0:a2bbf76ca734 121 mMotorDriveDout[ 1 ]->write( 0 );
inst 0:a2bbf76ca734 122 mMotorDriveDout[ 2 ]->write( 0 );
inst 0:a2bbf76ca734 123 mMotorDriveDout[ 3 ]->write( 1 );
inst 0:a2bbf76ca734 124 break;
inst 0:a2bbf76ca734 125
inst 0:a2bbf76ca734 126 case BRAKE:
inst 0:a2bbf76ca734 127 mMotorDriveDout[ 0 ]->write( 0 );
inst 0:a2bbf76ca734 128 mMotorDriveDout[ 1 ]->write( 0 );
inst 0:a2bbf76ca734 129 mMotorDriveDout[ 2 ]->write( 0 );
inst 0:a2bbf76ca734 130 mMotorDriveDout[ 3 ]->write( 0 );
inst 0:a2bbf76ca734 131 break;
inst 0:a2bbf76ca734 132
inst 0:a2bbf76ca734 133 case RELEASE:
inst 0:a2bbf76ca734 134 mMotorDriveDout[ 0 ]->write( 1 );
inst 0:a2bbf76ca734 135 mMotorDriveDout[ 1 ]->write( 1 );
inst 0:a2bbf76ca734 136 mMotorDriveDout[ 2 ]->write( 0 );
inst 0:a2bbf76ca734 137 mMotorDriveDout[ 3 ]->write( 0 );
inst 0:a2bbf76ca734 138 break;
inst 0:a2bbf76ca734 139
inst 0:a2bbf76ca734 140 default:
inst 0:a2bbf76ca734 141 mAction = RELEASE;
inst 0:a2bbf76ca734 142 mMotorDriveDout[ 0 ]->write( 1 );
inst 0:a2bbf76ca734 143 mMotorDriveDout[ 1 ]->write( 1 );
inst 0:a2bbf76ca734 144 mMotorDriveDout[ 2 ]->write( 0 );
inst 0:a2bbf76ca734 145 mMotorDriveDout[ 3 ]->write( 0 );
inst 0:a2bbf76ca734 146 break;
inst 0:a2bbf76ca734 147 }
inst 0:a2bbf76ca734 148 }
inst 0:a2bbf76ca734 149
inst 0:a2bbf76ca734 150 void YMotorDriverBase::setPercent( float p ){
inst 0:a2bbf76ca734 151 p = middle( 0.0f, p, 1.0f );
inst 0:a2bbf76ca734 152 mDuty = p * mMaxDuty + ( 1.0f - p ) * mMinDuty;
inst 0:a2bbf76ca734 153 }