FirstTest - Second revision of test code for Polyathalon sens…

Users » mpanetta » Code » FirstTest

Mike Panetta / Mbed 2 deprecated FirstTest

Second revision of test code for Polyathalon sensor board.

m3pi/m3pi.cpp@1:1e37eee8e1cf, 2011-11-05 (annotated)

Committer:: mpanetta
Date:: Sat Nov 05 21:10:10 2011 +0000
Revision:: 1:1e37eee8e1cf
Parent:: 0:13a7de7ce046

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	05 Nov 2011
Imports:	4
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	2
Followers:	2

m3pi/m3pi.cpp@1:1e37eee8e1cf, 2011-11-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning