m3pi_MazeSolver - This is work in progress - maze solving with m3pi…

Users » jonmarsh » Code » m3pi_MazeSolver

Jon Marsh / Mbed 2 deprecated m3pi_MazeSolver

This is work in progress - maze solving with m3pi robot. Not complete

m3pijon/m3pijon.cpp@0:6e52eae6412a, 2011-03-03 (annotated)

Committer:: jonmarsh
Date:: Thu Mar 03 09:58:58 2011 +0000
Revision:: 0:6e52eae6412a

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	03 Mar 2011
Imports:	163
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	2
Followers:	5

m3pijon/m3pijon.cpp@0:6e52eae6412a, 2011-03-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning