mbed_TANK_Kinect - Based on myBlueUSB reference ver. http://mbed.or…

Users » kenbumono » Code » mbed_TANK_Kinect

Yasuhiko YAMAMOTO / Mbed 2 deprecated mbed_TANK_Kinect

Based on myBlueUSB reference ver. http://mbed.org/users/networker/programs/myBlueUSB/lsm1ui

Dependencies: mbed myUSBHost AvailableMemory rfcomm myBlueUSB sdp

motordriver.cpp@0:8d8481ed6d49, 2011-07-05 (annotated)

Committer:: kenbumono
Date:: Tue Jul 05 08:25:59 2011 +0000
Revision:: 0:8d8481ed6d49

Who changed what in which revision?

User	Revision	Line number	New contents of line
kenbumono	0:8d8481ed6d49	1	/*motor driver libary modified from the following libary,
kenbumono	0:8d8481ed6d49	2	*
kenbumono	0:8d8481ed6d49	3	* mbed simple H-bridge motor controller
kenbumono	0:8d8481ed6d49	4	* Copyright (c) 2007-2010, sford
kenbumono	0:8d8481ed6d49	5	*
kenbumono	0:8d8481ed6d49	6	* by Christopher Hasler.
kenbumono	0:8d8481ed6d49	7	*
kenbumono	0:8d8481ed6d49	8	* from sford's libary,
kenbumono	0:8d8481ed6d49	9	*
kenbumono	0:8d8481ed6d49	10	* Permission is hereby granted, free of charge, to any person obtaining a copy
kenbumono	0:8d8481ed6d49	11	* of this software and associated documentation files (the "Software"), to deal
kenbumono	0:8d8481ed6d49	12	* in the Software without restriction, including without limitation the rights
kenbumono	0:8d8481ed6d49	13	* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
kenbumono	0:8d8481ed6d49	14	* copies of the Software, and to permit persons to whom the Software is
kenbumono	0:8d8481ed6d49	15	* furnished to do so, subject to the following conditions:
kenbumono	0:8d8481ed6d49	16	*
kenbumono	0:8d8481ed6d49	17	* The above copyright notice and this permission notice shall be included in
kenbumono	0:8d8481ed6d49	18	* all copies or substantial portions of the Software.
kenbumono	0:8d8481ed6d49	19	*
kenbumono	0:8d8481ed6d49	20	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
kenbumono	0:8d8481ed6d49	21	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
kenbumono	0:8d8481ed6d49	22	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
kenbumono	0:8d8481ed6d49	23	* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
kenbumono	0:8d8481ed6d49	24	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
kenbumono	0:8d8481ed6d49	25	* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
kenbumono	0:8d8481ed6d49	26	* THE SOFTWARE.
kenbumono	0:8d8481ed6d49	27	*/
kenbumono	0:8d8481ed6d49	28
kenbumono	0:8d8481ed6d49	29	#include "motordriver.h"
kenbumono	0:8d8481ed6d49	30
kenbumono	0:8d8481ed6d49	31	#include "mbed.h"
kenbumono	0:8d8481ed6d49	32
kenbumono	0:8d8481ed6d49	33	Motor::Motor(PinName pwm, PinName fwd, PinName rev, bool brakeable):
kenbumono	0:8d8481ed6d49	34	_pwm(pwm), _fwd(fwd), _rev(rev), _brakeable(brakeable), _sign(0) {
kenbumono	0:8d8481ed6d49	35
kenbumono	0:8d8481ed6d49	36	// Set initial condition of PWM
kenbumono	0:8d8481ed6d49	37	_pwm.period(0.001);
kenbumono	0:8d8481ed6d49	38	_pwm = 0;
kenbumono	0:8d8481ed6d49	39
kenbumono	0:8d8481ed6d49	40	// Initial condition of output enables
kenbumono	0:8d8481ed6d49	41	_fwd = 0;
kenbumono	0:8d8481ed6d49	42	_rev = 0;
kenbumono	0:8d8481ed6d49	43	}
kenbumono	0:8d8481ed6d49	44
kenbumono	0:8d8481ed6d49	45	float Motor::speed(float speed) {
kenbumono	0:8d8481ed6d49	46	float temp = 0;
kenbumono	0:8d8481ed6d49	47	if (_sign == 0) {
kenbumono	0:8d8481ed6d49	48	_fwd = (speed > 0.0);
kenbumono	0:8d8481ed6d49	49	_rev = (speed < 0.0);
kenbumono	0:8d8481ed6d49	50	temp = abs(speed);
kenbumono	0:8d8481ed6d49	51	_pwm = temp;
kenbumono	0:8d8481ed6d49	52	} else if (_sign == 1) {
kenbumono	0:8d8481ed6d49	53	if (speed < 0) {
kenbumono	0:8d8481ed6d49	54	_fwd = (speed > 0.0);
kenbumono	0:8d8481ed6d49	55	_rev = (speed < 0.0);
kenbumono	0:8d8481ed6d49	56	_pwm = 0;
kenbumono	0:8d8481ed6d49	57	temp = 0;
kenbumono	0:8d8481ed6d49	58	} else {
kenbumono	0:8d8481ed6d49	59	_fwd = (speed > 0.0);
kenbumono	0:8d8481ed6d49	60	_rev = (speed < 0.0);
kenbumono	0:8d8481ed6d49	61	temp = abs(speed);
kenbumono	0:8d8481ed6d49	62	_pwm = temp;
kenbumono	0:8d8481ed6d49	63	}
kenbumono	0:8d8481ed6d49	64	} else if (_sign == -1) {
kenbumono	0:8d8481ed6d49	65	if (speed > 0) {
kenbumono	0:8d8481ed6d49	66	_fwd = (speed > 0.0);
kenbumono	0:8d8481ed6d49	67	_rev = (speed < 0.0);
kenbumono	0:8d8481ed6d49	68	_pwm = 0;
kenbumono	0:8d8481ed6d49	69	temp = 0;
kenbumono	0:8d8481ed6d49	70	} else {
kenbumono	0:8d8481ed6d49	71	_fwd = (speed > 0.0);
kenbumono	0:8d8481ed6d49	72	_rev = (speed < 0.0);
kenbumono	0:8d8481ed6d49	73	temp = abs(speed);
kenbumono	0:8d8481ed6d49	74	_pwm = temp;
kenbumono	0:8d8481ed6d49	75	}
kenbumono	0:8d8481ed6d49	76	}
kenbumono	0:8d8481ed6d49	77	if (speed > 0)
kenbumono	0:8d8481ed6d49	78	_sign = 1;
kenbumono	0:8d8481ed6d49	79	else if (speed < 0) {
kenbumono	0:8d8481ed6d49	80	_sign = -1;
kenbumono	0:8d8481ed6d49	81	} else if (speed == 0) {
kenbumono	0:8d8481ed6d49	82	_sign = 0;
kenbumono	0:8d8481ed6d49	83	}
kenbumono	0:8d8481ed6d49	84	return temp;
kenbumono	0:8d8481ed6d49	85	}
kenbumono	0:8d8481ed6d49	86	// (additions)
kenbumono	0:8d8481ed6d49	87	void Motor::coast(void) {
kenbumono	0:8d8481ed6d49	88	_fwd = 0;
kenbumono	0:8d8481ed6d49	89	_rev = 0;
kenbumono	0:8d8481ed6d49	90	_pwm = 0;
kenbumono	0:8d8481ed6d49	91	_sign = 0;
kenbumono	0:8d8481ed6d49	92	}
kenbumono	0:8d8481ed6d49	93
kenbumono	0:8d8481ed6d49	94	float Motor::stop(float duty) {
kenbumono	0:8d8481ed6d49	95	if (_brakeable == 1) {
kenbumono	0:8d8481ed6d49	96	_fwd = 1;
kenbumono	0:8d8481ed6d49	97	_rev = 1;
kenbumono	0:8d8481ed6d49	98	_pwm = duty;
kenbumono	0:8d8481ed6d49	99	_sign = 0;
kenbumono	0:8d8481ed6d49	100	return duty;
kenbumono	0:8d8481ed6d49	101	} else
kenbumono	0:8d8481ed6d49	102	Motor::coast();
kenbumono	0:8d8481ed6d49	103	return -1; // error, can't brake
kenbumono	0:8d8481ed6d49	104	}
kenbumono	0:8d8481ed6d49	105
kenbumono	0:8d8481ed6d49	106	float Motor::state(void) const {
kenbumono	0:8d8481ed6d49	107	int fwd = _fwd.read();
kenbumono	0:8d8481ed6d49	108	int rev = _rev.read();
kenbumono	0:8d8481ed6d49	109	float pwm = _pwm.read();
kenbumono	0:8d8481ed6d49	110
kenbumono	0:8d8481ed6d49	111	if ((fwd == rev) && (pwm > 0)) {
kenbumono	0:8d8481ed6d49	112	return -2;//braking
kenbumono	0:8d8481ed6d49	113	} else if (pwm == 0) {
kenbumono	0:8d8481ed6d49	114	return 2;//coasting
kenbumono	0:8d8481ed6d49	115	} else if ((fwd == 0) && (rev == 1)) {
kenbumono	0:8d8481ed6d49	116	return -pwm;//reversing
kenbumono	0:8d8481ed6d49	117	} else if ((fwd == 1) && (rev == 0)) {
kenbumono	0:8d8481ed6d49	118	return pwm;//fowards
kenbumono	0:8d8481ed6d49	119	} else
kenbumono	0:8d8481ed6d49	120	return -3;//error
kenbumono	0:8d8481ed6d49	121	}
kenbumono	0:8d8481ed6d49	122
kenbumono	0:8d8481ed6d49	123	//test code, this demonstrates working motor drivers.
kenbumono	0:8d8481ed6d49	124
kenbumono	0:8d8481ed6d49	125	//Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break
kenbumono	0:8d8481ed6d49	126	/*
kenbumono	0:8d8481ed6d49	127	Motor A(p22, p7, p8, 1); // pwm, fwd, rev, can break
kenbumono	0:8d8481ed6d49	128	Motor B(p21, p5, p6, 1); // pwm, fwd, rev, can break
kenbumono	0:8d8481ed6d49	129	int main() {
kenbumono	0:8d8481ed6d49	130	for (float s=-1.0; s < 1.0 ; s += 0.01) {
kenbumono	0:8d8481ed6d49	131	//A.speed(s);
kenbumono	0:8d8481ed6d49	132	A.speed(s);
kenbumono	0:8d8481ed6d49	133	B.speed(s);
kenbumono	0:8d8481ed6d49	134	wait(0.02);
kenbumono	0:8d8481ed6d49	135	}
kenbumono	0:8d8481ed6d49	136	A.stop(0);
kenbumono	0:8d8481ed6d49	137	B.stop(0);
kenbumono	0:8d8481ed6d49	138	wait(1);
kenbumono	0:8d8481ed6d49	139	A.coast();
kenbumono	0:8d8481ed6d49	140	B.coast();
kenbumono	0:8d8481ed6d49	141	}
kenbumono	0:8d8481ed6d49	142	*/

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	05 Jul 2011
Imports:	15
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	6
Followers:	1

motordriver.cpp@0:8d8481ed6d49, 2011-07-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning