m3pi - | Mbed

Users » donde » Code » m3pi

Dependents: LPC1768_m3pi_LineFollower_PID

m3pi.cpp@0:4d063fa5d347, 2011-11-15 (annotated)

Committer:: donde
Date:: Tue Nov 15 02:14:45 2011 +0000
Revision:: 0:4d063fa5d347

Who changed what in which revision?

User	Revision	Line number	New contents of line
donde	0:4d063fa5d347	1	/* m3pi Library
donde	0:4d063fa5d347	2	*
donde	0:4d063fa5d347	3	* Copyright (c) 2007-2010 cstyles
donde	0:4d063fa5d347	4	*
donde	0:4d063fa5d347	5	* Permission is hereby granted, free of charge, to any person obtaining a copy
donde	0:4d063fa5d347	6	* of this software and associated documentation files (the "Software"), to deal
donde	0:4d063fa5d347	7	* in the Software without restriction, including without limitation the rights
donde	0:4d063fa5d347	8	* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
donde	0:4d063fa5d347	9	* copies of the Software, and to permit persons to whom the Software is
donde	0:4d063fa5d347	10	* furnished to do so, subject to the following conditions:
donde	0:4d063fa5d347	11	*
donde	0:4d063fa5d347	12	* The above copyright notice and this permission notice shall be included in
donde	0:4d063fa5d347	13	* all copies or substantial portions of the Software.
donde	0:4d063fa5d347	14	*
donde	0:4d063fa5d347	15	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
donde	0:4d063fa5d347	16	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
donde	0:4d063fa5d347	17	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
donde	0:4d063fa5d347	18	* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
donde	0:4d063fa5d347	19	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
donde	0:4d063fa5d347	20	* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
donde	0:4d063fa5d347	21	* THE SOFTWARE.
donde	0:4d063fa5d347	22	*/
donde	0:4d063fa5d347	23
donde	0:4d063fa5d347	24	#include "mbed.h"
donde	0:4d063fa5d347	25	#include "m3pi.h"
donde	0:4d063fa5d347	26
donde	0:4d063fa5d347	27	m3pi::m3pi(PinName nrst, PinName tx, PinName rx) : Stream("m3pi"), _nrst(nrst), _ser(tx, rx) {
donde	0:4d063fa5d347	28	_ser.baud(115200);
donde	0:4d063fa5d347	29	reset();
donde	0:4d063fa5d347	30	}
donde	0:4d063fa5d347	31
donde	0:4d063fa5d347	32	m3pi::m3pi() : Stream("m3pi"), _nrst(p23), _ser(p9, p10) {
donde	0:4d063fa5d347	33	_ser.baud(115200);
donde	0:4d063fa5d347	34	reset();
donde	0:4d063fa5d347	35	}
donde	0:4d063fa5d347	36
donde	0:4d063fa5d347	37
donde	0:4d063fa5d347	38	void m3pi::reset () {
donde	0:4d063fa5d347	39	_nrst = 0;
donde	0:4d063fa5d347	40	wait (0.01);
donde	0:4d063fa5d347	41	_nrst = 1;
donde	0:4d063fa5d347	42	wait (0.1);
donde	0:4d063fa5d347	43	}
donde	0:4d063fa5d347	44
donde	0:4d063fa5d347	45	void m3pi::left_motor (float speed) {
donde	0:4d063fa5d347	46	motor(0,speed);
donde	0:4d063fa5d347	47	}
donde	0:4d063fa5d347	48
donde	0:4d063fa5d347	49	void m3pi::right_motor (float speed) {
donde	0:4d063fa5d347	50	motor(1,speed);
donde	0:4d063fa5d347	51	}
donde	0:4d063fa5d347	52
donde	0:4d063fa5d347	53	void m3pi::forward (float speed) {
donde	0:4d063fa5d347	54	motor(0,speed);
donde	0:4d063fa5d347	55	motor(1,speed);
donde	0:4d063fa5d347	56	}
donde	0:4d063fa5d347	57
donde	0:4d063fa5d347	58	void m3pi::backward (float speed) {
donde	0:4d063fa5d347	59	motor(0,-1.0*speed);
donde	0:4d063fa5d347	60	motor(1,-1.0*speed);
donde	0:4d063fa5d347	61	}
donde	0:4d063fa5d347	62
donde	0:4d063fa5d347	63	void m3pi::left (float speed) {
donde	0:4d063fa5d347	64	motor(0,speed);
donde	0:4d063fa5d347	65	motor(1,-1.0*speed);
donde	0:4d063fa5d347	66	}
donde	0:4d063fa5d347	67
donde	0:4d063fa5d347	68	void m3pi::right (float speed) {
donde	0:4d063fa5d347	69	motor(0,-1.0*speed);
donde	0:4d063fa5d347	70	motor(1,speed);
donde	0:4d063fa5d347	71	}
donde	0:4d063fa5d347	72
donde	0:4d063fa5d347	73	void m3pi::stop (void) {
donde	0:4d063fa5d347	74	motor(0,0.0);
donde	0:4d063fa5d347	75	motor(1,0.0);
donde	0:4d063fa5d347	76	}
donde	0:4d063fa5d347	77
donde	0:4d063fa5d347	78	void m3pi::motor (int motor, float speed) {
donde	0:4d063fa5d347	79	char opcode = 0x0;
donde	0:4d063fa5d347	80	if (speed > 0.0) {
donde	0:4d063fa5d347	81	if (motor==1)
donde	0:4d063fa5d347	82	opcode = M1_FORWARD;
donde	0:4d063fa5d347	83	else
donde	0:4d063fa5d347	84	opcode = M2_FORWARD;
donde	0:4d063fa5d347	85	} else {
donde	0:4d063fa5d347	86	if (motor==1)
donde	0:4d063fa5d347	87	opcode = M1_BACKWARD;
donde	0:4d063fa5d347	88	else
donde	0:4d063fa5d347	89	opcode = M2_BACKWARD;
donde	0:4d063fa5d347	90	}
donde	0:4d063fa5d347	91	unsigned char arg = 0x7f * abs(speed);
donde	0:4d063fa5d347	92
donde	0:4d063fa5d347	93	_ser.putc(opcode);
donde	0:4d063fa5d347	94	_ser.putc(arg);
donde	0:4d063fa5d347	95	}
donde	0:4d063fa5d347	96
donde	0:4d063fa5d347	97	float m3pi::battery() {
donde	0:4d063fa5d347	98	_ser.putc(SEND_BATTERY_MILLIVOLTS);
donde	0:4d063fa5d347	99	char lowbyte = _ser.getc();
donde	0:4d063fa5d347	100	char hibyte = _ser.getc();
donde	0:4d063fa5d347	101	float v = ((lowbyte + (hibyte << 8))/1000.0);
donde	0:4d063fa5d347	102	return(v);
donde	0:4d063fa5d347	103	}
donde	0:4d063fa5d347	104
donde	0:4d063fa5d347	105	float m3pi::line_position() {
donde	0:4d063fa5d347	106	int pos = 0;
donde	0:4d063fa5d347	107	_ser.putc(SEND_LINE_POSITION);
donde	0:4d063fa5d347	108	pos = _ser.getc();
donde	0:4d063fa5d347	109	pos += _ser.getc() << 8;
donde	0:4d063fa5d347	110
donde	0:4d063fa5d347	111	float fpos = ((float)pos - 2048.0)/2048.0;
donde	0:4d063fa5d347	112	return(fpos);
donde	0:4d063fa5d347	113	}
donde	0:4d063fa5d347	114
donde	0:4d063fa5d347	115	char m3pi::sensor_auto_calibrate() {
donde	0:4d063fa5d347	116	_ser.putc(AUTO_CALIBRATE);
donde	0:4d063fa5d347	117	return(_ser.getc());
donde	0:4d063fa5d347	118	}
donde	0:4d063fa5d347	119
donde	0:4d063fa5d347	120
donde	0:4d063fa5d347	121	void m3pi::calibrate(void) {
donde	0:4d063fa5d347	122	_ser.putc(PI_CALIBRATE);
donde	0:4d063fa5d347	123	}
donde	0:4d063fa5d347	124
donde	0:4d063fa5d347	125	void m3pi::reset_calibration() {
donde	0:4d063fa5d347	126	_ser.putc(LINE_SENSORS_RESET_CALIBRATION);
donde	0:4d063fa5d347	127	}
donde	0:4d063fa5d347	128
donde	0:4d063fa5d347	129	void m3pi::PID_start(int max_speed, int a, int b, int c, int d) {
donde	0:4d063fa5d347	130	_ser.putc(SET_PID);
donde	0:4d063fa5d347	131	_ser.putc(max_speed);
donde	0:4d063fa5d347	132	_ser.putc(a);
donde	0:4d063fa5d347	133	_ser.putc(b);
donde	0:4d063fa5d347	134	_ser.putc(c);
donde	0:4d063fa5d347	135	_ser.putc(d);
donde	0:4d063fa5d347	136	}
donde	0:4d063fa5d347	137
donde	0:4d063fa5d347	138	void m3pi::PID_stop() {
donde	0:4d063fa5d347	139	_ser.putc(STOP_PID);
donde	0:4d063fa5d347	140	}
donde	0:4d063fa5d347	141
donde	0:4d063fa5d347	142	float m3pi::pot_voltage(void) {
donde	0:4d063fa5d347	143	int volt = 0;
donde	0:4d063fa5d347	144	_ser.putc(SEND_TRIMPOT);
donde	0:4d063fa5d347	145	volt = _ser.getc();
donde	0:4d063fa5d347	146	volt += _ser.getc() << 8;
donde	0:4d063fa5d347	147	return(volt);
donde	0:4d063fa5d347	148	}
donde	0:4d063fa5d347	149
donde	0:4d063fa5d347	150
donde	0:4d063fa5d347	151	void m3pi::leds(int val) {
donde	0:4d063fa5d347	152
donde	0:4d063fa5d347	153	BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13);
donde	0:4d063fa5d347	154	_leds = val;
donde	0:4d063fa5d347	155	}
donde	0:4d063fa5d347	156
donde	0:4d063fa5d347	157
donde	0:4d063fa5d347	158	void m3pi::locate(int x, int y) {
donde	0:4d063fa5d347	159	_ser.putc(DO_LCD_GOTO_XY);
donde	0:4d063fa5d347	160	_ser.putc(x);
donde	0:4d063fa5d347	161	_ser.putc(y);
donde	0:4d063fa5d347	162	}
donde	0:4d063fa5d347	163
donde	0:4d063fa5d347	164	void m3pi::cls(void) {
donde	0:4d063fa5d347	165	_ser.putc(DO_CLEAR);
donde	0:4d063fa5d347	166	}
donde	0:4d063fa5d347	167
donde	0:4d063fa5d347	168	int m3pi::print (char* text, int length) {
donde	0:4d063fa5d347	169	_ser.putc(DO_PRINT);
donde	0:4d063fa5d347	170	_ser.putc(length);
donde	0:4d063fa5d347	171	for (int i = 0 ; i < length ; i++) {
donde	0:4d063fa5d347	172	_ser.putc(text[i]);
donde	0:4d063fa5d347	173	}
donde	0:4d063fa5d347	174	return(0);
donde	0:4d063fa5d347	175	}
donde	0:4d063fa5d347	176
donde	0:4d063fa5d347	177	void m3pi::play (char* music, int length) {
donde	0:4d063fa5d347	178	// The default 3pi serial slave program
donde	0:4d063fa5d347	179	// will not accept music strings longer than 100.
donde	0:4d063fa5d347	180	if (length > 100)
donde	0:4d063fa5d347	181	length = 100;
donde	0:4d063fa5d347	182	_ser.putc(DO_PLAY);
donde	0:4d063fa5d347	183	_ser.putc(length);
donde	0:4d063fa5d347	184	for (int i = 0 ; i < length ; i++) {
donde	0:4d063fa5d347	185	_ser.putc(music[i]);
donde	0:4d063fa5d347	186	}
donde	0:4d063fa5d347	187	}
donde	0:4d063fa5d347	188
donde	0:4d063fa5d347	189	int m3pi::_putc (int c) {
donde	0:4d063fa5d347	190	_ser.putc(DO_PRINT);
donde	0:4d063fa5d347	191	_ser.putc(0x1);
donde	0:4d063fa5d347	192	_ser.putc(c);
donde	0:4d063fa5d347	193	wait (0.001);
donde	0:4d063fa5d347	194	return(c);
donde	0:4d063fa5d347	195	}
donde	0:4d063fa5d347	196
donde	0:4d063fa5d347	197	int m3pi::_getc (void) {
donde	0:4d063fa5d347	198	char r = 0;
donde	0:4d063fa5d347	199	return(r);
donde	0:4d063fa5d347	200	}
donde	0:4d063fa5d347	201
donde	0:4d063fa5d347	202	int m3pi::putc (int c) {
donde	0:4d063fa5d347	203	return(_ser.putc(c));
donde	0:4d063fa5d347	204	}
donde	0:4d063fa5d347	205
donde	0:4d063fa5d347	206	int m3pi::getc (void) {
donde	0:4d063fa5d347	207	return(_ser.getc());
donde	0:4d063fa5d347	208	}
donde	0:4d063fa5d347	209
donde	0:4d063fa5d347	210
donde	0:4d063fa5d347	211
donde	0:4d063fa5d347	212
donde	0:4d063fa5d347	213	#ifdef MBED_RPC
donde	0:4d063fa5d347	214	const rpc_method *m3pi::get_rpc_methods() {
donde	0:4d063fa5d347	215	static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> },
donde	0:4d063fa5d347	216	{ "backward", rpc_method_caller<m3pi, float, &m3pi::backward> },
donde	0:4d063fa5d347	217	{ "left", rpc_method_caller<m3pi, float, &m3pi::left> },
donde	0:4d063fa5d347	218	{ "right", rpc_method_caller<m3pi, float, &m3pi::right> },
donde	0:4d063fa5d347	219	{ "stop", rpc_method_caller<m3pi, &m3pi::stop> },
donde	0:4d063fa5d347	220	{ "left_motor", rpc_method_caller<m3pi, float, &m3pi::left_motor> },
donde	0:4d063fa5d347	221	{ "right_motor", rpc_method_caller<m3pi, float, &m3pi::right_motor> },
donde	0:4d063fa5d347	222	{ "battery", rpc_method_caller<float, m3pi, &m3pi::battery> },
donde	0:4d063fa5d347	223	{ "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> },
donde	0:4d063fa5d347	224	{ "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> },
donde	0:4d063fa5d347	225
donde	0:4d063fa5d347	226
donde	0:4d063fa5d347	227	RPC_METHOD_SUPER(Base)
donde	0:4d063fa5d347	228	};
donde	0:4d063fa5d347	229	return rpc_methods;
donde	0:4d063fa5d347	230	}
donde	0:4d063fa5d347	231	#endif

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	15 Nov 2011
Imports:	649
Forks:	0
Commits:	1
Dependents:	1
Dependencies:	0
Followers:	10

m3pi.cpp@0:4d063fa5d347, 2011-11-15 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning