m3pi_sonar - Testcode for LVC Robot club

Users » jonmarsh » Code » m3pi_sonar

Jon Marsh / Mbed 2 deprecated m3pi_sonar

Testcode for LVC Robot club

m3pi/m3pi.cpp@1:fe52aa73cd6a, 2012-04-23 (annotated)

Committer:: jonmarsh
Date:: Mon Apr 23 09:32:50 2012 +0000
Revision:: 1:fe52aa73cd6a
Parent:: 0:f4922a2a0292

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	16 Apr 2012
Imports:	7
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	1
Followers:	1

m3pi/m3pi.cpp@1:fe52aa73cd6a, 2012-04-23 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning