YMotorDriverBase -

Users » inst » Code » YMotorDriverBase

YMotorDriverBase.cpp@1:c8ed08beefb9, 2015-10-14 (annotated)

Committer:: inst
Date:: Wed Oct 14 06:04:34 2015 +0000
Revision:: 1:c8ed08beefb9
Parent:: 0:a2bbf76ca734
Child:: 2:871d1f6d311e

Who changed what in which revision?

User	Revision	Line number	New 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.25f // supplier
inst	1:c8ed08beefb9	27	0.22f // supplier
inst	1:c8ed08beefb9	28	};
inst	1:c8ed08beefb9	29	const float YMotorDriverBase::mMaxDutyList[] = {
inst	1:c8ed08beefb9	30	0.85f, // steering
inst	1:c8ed08beefb9	31	0.85f, //
inst	1:c8ed08beefb9	32	0.85f, //
inst	1:c8ed08beefb9	33	1.0f, // shooter
inst	1:c8ed08beefb9	34	0.9f, // general
inst	1:c8ed08beefb9	35	0.50f // supplier
inst	1:c8ed08beefb9	36	};
inst	0:a2bbf76ca734	37
inst	1:c8ed08beefb9	38	YMotorDriverBase::YMotorDriverBase( char address ) :
inst	1:c8ed08beefb9	39	mAddress( address ),
inst	1:c8ed08beefb9	40	mMaxDuty( mMaxDutyList[ YMotorDriver::GENERAL_ID ] ), mMinDuty( mMinDutyList[ YMotorDriver::GENERAL_ID ] ){
inst	1:c8ed08beefb9	41
inst	1:c8ed08beefb9	42	init();
inst	1:c8ed08beefb9	43	}
inst	1:c8ed08beefb9	44
inst	1:c8ed08beefb9	45	YMotorDriverBase::YMotorDriverBase( char address, int id ) :
inst	1:c8ed08beefb9	46	mAddress( address ), mMaxDuty( mMaxDutyList[ id ] ), mMinDuty( mMinDutyList[ id ] ){
inst	1:c8ed08beefb9	47	init();
inst	1:c8ed08beefb9	48	}
inst	1:c8ed08beefb9	49
inst	1:c8ed08beefb9	50	YMotorDriverBase::YMotorDriverBase( char address, float maxDuty, float minDuty ) :
inst	1:c8ed08beefb9	51	mAddress( address ), mMaxDuty( maxDuty ), mMinDuty( minDuty ){
inst	1:c8ed08beefb9	52	init();
inst	1:c8ed08beefb9	53	}
inst	1:c8ed08beefb9	54
inst	1:c8ed08beefb9	55	void YMotorDriverBase::init(){
inst	0:a2bbf76ca734	56	mI2C = new I2CSlave( mI2CPinName[ 0 ], mI2CPinName[ 1 ] );
inst	0:a2bbf76ca734	57	mI2C->address( mAddress );
inst	0:a2bbf76ca734	58
inst	0:a2bbf76ca734	59	for ( int i = 0; i < 4; ++i ){
inst	0:a2bbf76ca734	60	mMotorDriveDout[ i ] = new DigitalOut( mMotorDriveDoutPinName[ i ] );
inst	0:a2bbf76ca734	61	}
inst	0:a2bbf76ca734	62
inst	0:a2bbf76ca734	63	mAction = RELEASE;
inst	0:a2bbf76ca734	64
inst	0:a2bbf76ca734	65	mMotorDrivePwm = new PWMOut( mMotorDrivePwmPinName );
inst	0:a2bbf76ca734	66	mMotorDrivePwm->setPeriod_us( mPwmCycle_us );
inst	0:a2bbf76ca734	67	mMotorDrivePwm->setDuty( mMinDuty );
inst	0:a2bbf76ca734	68
inst	0:a2bbf76ca734	69	mLED = new DigitalOut( mLEDPinName, 0 );
inst	0:a2bbf76ca734	70
inst	0:a2bbf76ca734	71	write();
inst	0:a2bbf76ca734	72	}
inst	0:a2bbf76ca734	73
inst	0:a2bbf76ca734	74	YMotorDriverBase::~YMotorDriverBase(){
inst	0:a2bbf76ca734	75	delete mI2C;
inst	0:a2bbf76ca734	76	}
inst	0:a2bbf76ca734	77
inst	0:a2bbf76ca734	78	void YMotorDriverBase::update(){
inst	1:c8ed08beefb9	79	updateI2CSlave();
inst	1:c8ed08beefb9	80	updateSpecial();
inst	1:c8ed08beefb9	81	write();
inst	1:c8ed08beefb9	82	}
inst	1:c8ed08beefb9	83
inst	1:c8ed08beefb9	84	void YMotorDriverBase::updateI2CSlave(){
inst	0:a2bbf76ca734	85	switch ( mI2C->receive() ){
inst	0:a2bbf76ca734	86	case I2CSlave::ReadAddressed:{
inst	0:a2bbf76ca734	87	char buf[] = { mMinDuty * 255.0f, mMaxDuty * 255.0f };
inst	0:a2bbf76ca734	88	mI2C->write( buf, 2 );
inst	0:a2bbf76ca734	89	break;
inst	0:a2bbf76ca734	90	}
inst	0:a2bbf76ca734	91	case I2CSlave::WriteGeneral:
inst	0:a2bbf76ca734	92	break;
inst	0:a2bbf76ca734	93
inst	0:a2bbf76ca734	94	case I2CSlave::WriteAddressed:{
inst	0:a2bbf76ca734	95	char buf[ 2 ];
inst	0:a2bbf76ca734	96	mI2C->read( buf, 2 );
inst	0:a2bbf76ca734	97	// 初めの1Byteはモータの動作を指定する
inst	0:a2bbf76ca734	98	mAction = static_cast< MotorAction >( buf[ 0 ] );
inst	0:a2bbf76ca734	99	// 次の1Byteは0~255でDutyを指定する
inst	0:a2bbf76ca734	100	mDuty = static_cast< float >( buf[ 1 ] ) / 255.0f;
inst	0:a2bbf76ca734	101	break;
inst	0:a2bbf76ca734	102	}
inst	0:a2bbf76ca734	103
inst	0:a2bbf76ca734	104	case I2CSlave::NoData:
inst	0:a2bbf76ca734	105	break;
inst	0:a2bbf76ca734	106	}
inst	0:a2bbf76ca734	107	}
inst	1:c8ed08beefb9	108
inst	0:a2bbf76ca734	109	void YMotorDriverBase::write(){
inst	0:a2bbf76ca734	110	updatePwmDuty();
inst	0:a2bbf76ca734	111
inst	0:a2bbf76ca734	112	switch ( mAction ){
inst	0:a2bbf76ca734	113	case FORWARD:
inst	0:a2bbf76ca734	114	mMotorDriveDout[ 0 ]->write( 0 );
inst	0:a2bbf76ca734	115	mMotorDriveDout[ 1 ]->write( 1 );
inst	0:a2bbf76ca734	116	mMotorDriveDout[ 2 ]->write( 1 );
inst	0:a2bbf76ca734	117	mMotorDriveDout[ 3 ]->write( 0 );
inst	0:a2bbf76ca734	118	break;
inst	0:a2bbf76ca734	119
inst	0:a2bbf76ca734	120	case REVERSE:
inst	0:a2bbf76ca734	121	mMotorDriveDout[ 0 ]->write( 1 );
inst	0:a2bbf76ca734	122	mMotorDriveDout[ 1 ]->write( 0 );
inst	0:a2bbf76ca734	123	mMotorDriveDout[ 2 ]->write( 0 );
inst	0:a2bbf76ca734	124	mMotorDriveDout[ 3 ]->write( 1 );
inst	0:a2bbf76ca734	125	break;
inst	0:a2bbf76ca734	126
inst	0:a2bbf76ca734	127	case BRAKE:
inst	0:a2bbf76ca734	128	mMotorDriveDout[ 0 ]->write( 0 );
inst	0:a2bbf76ca734	129	mMotorDriveDout[ 1 ]->write( 0 );
inst	0:a2bbf76ca734	130	mMotorDriveDout[ 2 ]->write( 0 );
inst	0:a2bbf76ca734	131	mMotorDriveDout[ 3 ]->write( 0 );
inst	0:a2bbf76ca734	132	break;
inst	0:a2bbf76ca734	133
inst	0:a2bbf76ca734	134	case RELEASE:
inst	0:a2bbf76ca734	135	mMotorDriveDout[ 0 ]->write( 1 );
inst	0:a2bbf76ca734	136	mMotorDriveDout[ 1 ]->write( 1 );
inst	0:a2bbf76ca734	137	mMotorDriveDout[ 2 ]->write( 0 );
inst	0:a2bbf76ca734	138	mMotorDriveDout[ 3 ]->write( 0 );
inst	0:a2bbf76ca734	139	break;
inst	0:a2bbf76ca734	140
inst	0:a2bbf76ca734	141	default:
inst	0:a2bbf76ca734	142	mAction = RELEASE;
inst	0:a2bbf76ca734	143	mMotorDriveDout[ 0 ]->write( 1 );
inst	0:a2bbf76ca734	144	mMotorDriveDout[ 1 ]->write( 1 );
inst	0:a2bbf76ca734	145	mMotorDriveDout[ 2 ]->write( 0 );
inst	0:a2bbf76ca734	146	mMotorDriveDout[ 3 ]->write( 0 );
inst	0:a2bbf76ca734	147	break;
inst	0:a2bbf76ca734	148	}
inst	0:a2bbf76ca734	149	}
inst	0:a2bbf76ca734	150
inst	0:a2bbf76ca734	151	void YMotorDriverBase::updatePwmDuty(){
inst	0:a2bbf76ca734	152	static float prevDuty = mMinDuty;
inst	0:a2bbf76ca734	153	float d = middle( mMinDuty, mDuty, mMaxDuty );
inst	0:a2bbf76ca734	154
inst	0:a2bbf76ca734	155	if ( d != prevDuty ){
inst	0:a2bbf76ca734	156	mMotorDrivePwm->setDuty( d );
inst	0:a2bbf76ca734	157	}
inst	0:a2bbf76ca734	158	prevDuty = d;
inst	0:a2bbf76ca734	159	}
inst	0:a2bbf76ca734	160
inst	0:a2bbf76ca734	161	void YMotorDriverBase::setPercent( float p ){
inst	0:a2bbf76ca734	162	p = middle( 0.0f, p, 1.0f );
inst	0:a2bbf76ca734	163	mDuty = p * mMaxDuty + ( 1.0f - p ) * mMinDuty;
inst	0:a2bbf76ca734	164	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	14 Oct 2015
Imports:	0
Forks:	0
Commits:	4
Dependents:	1
Dependencies:	0
Followers:	1

YMotorDriverBase.cpp@1:c8ed08beefb9, 2015-10-14 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning