controller_with_backup - Made code public

Users » yxyang » Code » controller_with_backup

Yuxiang Yang / Mbed 2 deprecated controller_with_backup

Made code public

Dependencies: MPU6050IMU_1 TSL1401CL_1 mbed

main.cpp@0:ba0a5e86259f, 2017-05-30 (annotated)

Committer:: yxyang
Date:: Tue May 30 06:55:26 2017 +0000
Revision:: 0:ba0a5e86259f

Who changed what in which revision?

User	Revision	Line number	New contents of line
yxyang	0:ba0a5e86259f	1	/*
yxyang	0:ba0a5e86259f	2	* A sample path following program for the Zumy board.
yxyang	0:ba0a5e86259f	3	* Author: Galen Savidge
yxyang	0:ba0a5e86259f	4	*
yxyang	0:ba0a5e86259f	5	* Behavior:
yxyang	0:ba0a5e86259f	6	* Zumy follows the edge of the line until no line is found for LINE_END_TIME
yxyang	0:ba0a5e86259f	7	* cycles. Line position is recorded every frame as an 8 bit unsigned int. The
yxyang	0:ba0a5e86259f	8	* general area of the line is shown on the 4 LEDs on the mbed board. If a line
yxyang	0:ba0a5e86259f	9	* is not found all 4 LEDs are turned on. The Zumy stops and ine position data
yxyang	0:ba0a5e86259f	10	* is output to serial once the end of the track is reached.
yxyang	0:ba0a5e86259f	11	*/
yxyang	0:ba0a5e86259f	12
yxyang	0:ba0a5e86259f	13	#include "TSL1401CL.h"
yxyang	0:ba0a5e86259f	14	#include "mbed.h"
yxyang	0:ba0a5e86259f	15
yxyang	0:ba0a5e86259f	16	/*** Constants ****/
yxyang	0:ba0a5e86259f	17	#define CAM_INTEGRATION_TIME 80
yxyang	0:ba0a5e86259f	18
yxyang	0:ba0a5e86259f	19	// Higher line threshold -> the sensor will only recognize larger changes in
yxyang	0:ba0a5e86259f	20	// brightness as a line edge
yxyang	0:ba0a5e86259f	21	#define LINE_THRESHOLD 000
yxyang	0:ba0a5e86259f	22	#define LINE_PRECISION 2
yxyang	0:ba0a5e86259f	23	#define LINE_CROP_AMOUNT 4
yxyang	0:ba0a5e86259f	24
yxyang	0:ba0a5e86259f	25	// These constants define the base pwm across the motors and how much the
yxyang	0:ba0a5e86259f	26	// controller
yxyang	0:ba0a5e86259f	27	// adjusts based on position of the line relative to the sensor
yxyang	0:ba0a5e86259f	28	#define SPEED_PWM 0.25
yxyang	0:ba0a5e86259f	29	#define TURN_SENS_INNER 1.5F
yxyang	0:ba0a5e86259f	30	#define TURN_SENS_OUTER 0.5F
yxyang	0:ba0a5e86259f	31
yxyang	0:ba0a5e86259f	32	// Defines data
yxyang	0:ba0a5e86259f	33	#define LINE_HIST_SIZE 12000
yxyang	0:ba0a5e86259f	34	#define RETRACT_START_TIME 50
yxyang	0:ba0a5e86259f	35	#define LINE_END_TIME 1000
yxyang	0:ba0a5e86259f	36	#define SMOOTHING_FACTOR 0.5
yxyang	0:ba0a5e86259f	37
yxyang	0:ba0a5e86259f	38	// Sensor pins
yxyang	0:ba0a5e86259f	39	#define clk p16
yxyang	0:ba0a5e86259f	40	#define si p17
yxyang	0:ba0a5e86259f	41	#define adc p18
yxyang	0:ba0a5e86259f	42
yxyang	0:ba0a5e86259f	43	// Motors -- As labelled on the Zumy mbed board
yxyang	0:ba0a5e86259f	44	PwmOut m1_fwd (p21);
yxyang	0:ba0a5e86259f	45	PwmOut m1_back (p22);
yxyang	0:ba0a5e86259f	46
yxyang	0:ba0a5e86259f	47	PwmOut m2_fwd (p23);
yxyang	0:ba0a5e86259f	48	PwmOut m2_back (p24);
yxyang	0:ba0a5e86259f	49
yxyang	0:ba0a5e86259f	50	// LEDs
yxyang	0:ba0a5e86259f	51	DigitalOut led1 (LED1);
yxyang	0:ba0a5e86259f	52	DigitalOut led2 (LED2);
yxyang	0:ba0a5e86259f	53	DigitalOut led3 (LED3);
yxyang	0:ba0a5e86259f	54	DigitalOut led4 (LED4);
yxyang	0:ba0a5e86259f	55
yxyang	0:ba0a5e86259f	56	// USB serial
yxyang	0:ba0a5e86259f	57	Serial pc (USBTX, USBRX);
yxyang	0:ba0a5e86259f	58
yxyang	0:ba0a5e86259f	59	// Data storage
yxyang	0:ba0a5e86259f	60	uint16_t line_hist[LINE_HIST_SIZE];
yxyang	0:ba0a5e86259f	61	uint16_t hist_index;
yxyang	0:ba0a5e86259f	62
yxyang	0:ba0a5e86259f	63	/*** Helper functions ***/
yxyang	0:ba0a5e86259f	64	float max (float a, float b);
yxyang	0:ba0a5e86259f	65
yxyang	0:ba0a5e86259f	66	// Sets the 4 LEDS on the mbed board to the four given binary values
yxyang	0:ba0a5e86259f	67	void setLEDs (uint16_t a, uint16_t b, uint16_t c, uint16_t d);
yxyang	0:ba0a5e86259f	68
yxyang	0:ba0a5e86259f	69	// Based on steerPointTurns; the outer wheel moves faster the farther the line
yxyang	0:ba0a5e86259f	70	// is from center
yxyang	0:ba0a5e86259f	71	void steerImprovedPointTurns (int16_t line_pos);
yxyang	0:ba0a5e86259f	72
yxyang	0:ba0a5e86259f	73	void steerImprovedPointTurnsBackward (int16_t line_pos);
yxyang	0:ba0a5e86259f	74
yxyang	0:ba0a5e86259f	75	int
yxyang	0:ba0a5e86259f	76	main ()
yxyang	0:ba0a5e86259f	77	{
yxyang	0:ba0a5e86259f	78	/******** SENSOR SETUP ********/
yxyang	0:ba0a5e86259f	79	TSL1401CL cam (clk, si, adc);
yxyang	0:ba0a5e86259f	80	cam.setIntegrationTime (CAM_INTEGRATION_TIME);
yxyang	0:ba0a5e86259f	81
yxyang	0:ba0a5e86259f	82	int16_t line_pos = -1;
yxyang	0:ba0a5e86259f	83	int16_t line_pos_previous = -1;
yxyang	0:ba0a5e86259f	84	int16_t steer_pos = -1;
yxyang	0:ba0a5e86259f	85	uint16_t line_lost_time = 0;
yxyang	0:ba0a5e86259f	86
yxyang	0:ba0a5e86259f	87	hist_index = 0;
yxyang	0:ba0a5e86259f	88
yxyang	0:ba0a5e86259f	89	// Read garbage data
yxyang	0:ba0a5e86259f	90	while (!cam.integrationReady ())
yxyang	0:ba0a5e86259f	91	;
yxyang	0:ba0a5e86259f	92	cam.read ();
yxyang	0:ba0a5e86259f	93
yxyang	0:ba0a5e86259f	94	while (1)
yxyang	0:ba0a5e86259f	95	{
yxyang	0:ba0a5e86259f	96	/*** Read line sensor ***/
yxyang	0:ba0a5e86259f	97	while (!cam.integrationReady ())
yxyang	0:ba0a5e86259f	98	; // Wait for integration
yxyang	0:ba0a5e86259f	99	cam.read ();
yxyang	0:ba0a5e86259f	100
yxyang	0:ba0a5e86259f	101	/*** Line following loop ***/
yxyang	0:ba0a5e86259f	102
yxyang	0:ba0a5e86259f	103	line_pos = cam.findLineEdge (LINE_THRESHOLD, LINE_PRECISION,
yxyang	0:ba0a5e86259f	104	LINE_CROP_AMOUNT);
yxyang	0:ba0a5e86259f	105
yxyang	0:ba0a5e86259f	106	if (line_pos != -1)
yxyang	0:ba0a5e86259f	107	{ // On the line
yxyang	0:ba0a5e86259f	108	// Set LEDs based on the position of the line
yxyang	0:ba0a5e86259f	109	if (line_pos < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 4)
yxyang	0:ba0a5e86259f	110	{
yxyang	0:ba0a5e86259f	111	setLEDs (1, 0, 0, 0);
yxyang	0:ba0a5e86259f	112	}
yxyang	0:ba0a5e86259f	113	else if (line_pos
yxyang	0:ba0a5e86259f	114	< (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 2)
yxyang	0:ba0a5e86259f	115	{
yxyang	0:ba0a5e86259f	116	setLEDs (0, 1, 0, 0);
yxyang	0:ba0a5e86259f	117	}
yxyang	0:ba0a5e86259f	118	else if (line_pos
yxyang	0:ba0a5e86259f	119	< (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) * 3 / 4)
yxyang	0:ba0a5e86259f	120	{
yxyang	0:ba0a5e86259f	121	setLEDs (0, 0, 1, 0);
yxyang	0:ba0a5e86259f	122	}
yxyang	0:ba0a5e86259f	123	else
yxyang	0:ba0a5e86259f	124	{
yxyang	0:ba0a5e86259f	125	setLEDs (0, 0, 0, 1);
yxyang	0:ba0a5e86259f	126	}
yxyang	0:ba0a5e86259f	127
yxyang	0:ba0a5e86259f	128	// Record the line position
yxyang	0:ba0a5e86259f	129	line_hist[hist_index] = line_pos;
yxyang	0:ba0a5e86259f	130	hist_index++;
yxyang	0:ba0a5e86259f	131	line_lost_time = 0;
yxyang	0:ba0a5e86259f	132
yxyang	0:ba0a5e86259f	133	if (steer_pos == -1)
yxyang	0:ba0a5e86259f	134	{
yxyang	0:ba0a5e86259f	135	steer_pos = line_pos;
yxyang	0:ba0a5e86259f	136	}
yxyang	0:ba0a5e86259f	137	else
yxyang	0:ba0a5e86259f	138	{
yxyang	0:ba0a5e86259f	139	steer_pos = line_pos * (1 - SMOOTHING_FACTOR)
yxyang	0:ba0a5e86259f	140	+ steer_pos * SMOOTHING_FACTOR;
yxyang	0:ba0a5e86259f	141	}
yxyang	0:ba0a5e86259f	142
yxyang	0:ba0a5e86259f	143	// Steer the vehicle
yxyang	0:ba0a5e86259f	144	steerImprovedPointTurns (steer_pos);
yxyang	0:ba0a5e86259f	145	line_pos_previous = line_pos;
yxyang	0:ba0a5e86259f	146	}
yxyang	0:ba0a5e86259f	147	else
yxyang	0:ba0a5e86259f	148	{ // Lost the line
yxyang	0:ba0a5e86259f	149	line_lost_time++;
yxyang	0:ba0a5e86259f	150	if (line_lost_time >= LINE_END_TIME)
yxyang	0:ba0a5e86259f	151	{
yxyang	0:ba0a5e86259f	152	// Line end; transmit data to PC
yxyang	0:ba0a5e86259f	153	setLEDs (1, 1, 1, 1);
yxyang	0:ba0a5e86259f	154	m1_fwd.write (0);
yxyang	0:ba0a5e86259f	155	m2_fwd.write (0);
yxyang	0:ba0a5e86259f	156	m1_back.write (0);
yxyang	0:ba0a5e86259f	157	m2_back.write (0);
yxyang	0:ba0a5e86259f	158	while (!pc.readable ())
yxyang	0:ba0a5e86259f	159	;
yxyang	0:ba0a5e86259f	160	pc.printf ("----- Start line data -----\n");
yxyang	0:ba0a5e86259f	161	for (int i = 0; i <= hist_index; i++)
yxyang	0:ba0a5e86259f	162	{
yxyang	0:ba0a5e86259f	163	pc.printf ("%d\n", line_hist[i]);
yxyang	0:ba0a5e86259f	164	}
yxyang	0:ba0a5e86259f	165	while (1)
yxyang	0:ba0a5e86259f	166	;
yxyang	0:ba0a5e86259f	167	}
yxyang	0:ba0a5e86259f	168	else if (abs (steer_pos - TSL1401CL_PIXEL_COUNT / 2)
yxyang	0:ba0a5e86259f	169	> TSL1401CL_PIXEL_COUNT / 4)
yxyang	0:ba0a5e86259f	170	{
yxyang	0:ba0a5e86259f	171	setLEDs (1, 0, 0, 1);
yxyang	0:ba0a5e86259f	172	// Steer at maximum turn angle towards the last known line
yxyang	0:ba0a5e86259f	173	// direction
yxyang	0:ba0a5e86259f	174	if (steer_pos >= TSL1401CL_PIXEL_COUNT / 2)
yxyang	0:ba0a5e86259f	175	{
yxyang	0:ba0a5e86259f	176	steerImprovedPointTurns (TSL1401CL_PIXEL_COUNT - 1
yxyang	0:ba0a5e86259f	177	- LINE_CROP_AMOUNT);
yxyang	0:ba0a5e86259f	178	// line_hist[hist_index] = TSL1401CL_PIXEL_COUNT - 1 -
yxyang	0:ba0a5e86259f	179	// LINE_CROP_AMOUNT;
yxyang	0:ba0a5e86259f	180	}
yxyang	0:ba0a5e86259f	181	else
yxyang	0:ba0a5e86259f	182	{
yxyang	0:ba0a5e86259f	183	steerImprovedPointTurns (LINE_CROP_AMOUNT);
yxyang	0:ba0a5e86259f	184	// line_hist[hist_index] = LINE_CROP_AMOUNT;
yxyang	0:ba0a5e86259f	185	}
yxyang	0:ba0a5e86259f	186	}
yxyang	0:ba0a5e86259f	187	else if (line_lost_time > RETRACT_START_TIME)
yxyang	0:ba0a5e86259f	188	{
yxyang	0:ba0a5e86259f	189	setLEDs (0, 1, 1, 0);
yxyang	0:ba0a5e86259f	190	steerImprovedPointTurnsBackward (TSL1401CL_PIXEL_COUNT / 2);
yxyang	0:ba0a5e86259f	191	wait_ms (50);
yxyang	0:ba0a5e86259f	192	}
yxyang	0:ba0a5e86259f	193	else
yxyang	0:ba0a5e86259f	194	{
yxyang	0:ba0a5e86259f	195	setLEDs (0, 0, 0, 0);
yxyang	0:ba0a5e86259f	196	}
yxyang	0:ba0a5e86259f	197	line_hist[hist_index] = 0;
yxyang	0:ba0a5e86259f	198	hist_index++;
yxyang	0:ba0a5e86259f	199	}
yxyang	0:ba0a5e86259f	200	if (hist_index > LINE_HIST_SIZE)
yxyang	0:ba0a5e86259f	201	{
yxyang	0:ba0a5e86259f	202	hist_index = 0;
yxyang	0:ba0a5e86259f	203	}
yxyang	0:ba0a5e86259f	204	}
yxyang	0:ba0a5e86259f	205	}
yxyang	0:ba0a5e86259f	206
yxyang	0:ba0a5e86259f	207	float
yxyang	0:ba0a5e86259f	208	max (float a, float b)
yxyang	0:ba0a5e86259f	209	{
yxyang	0:ba0a5e86259f	210	if (a >= b)
yxyang	0:ba0a5e86259f	211	return a;
yxyang	0:ba0a5e86259f	212	return b;
yxyang	0:ba0a5e86259f	213	}
yxyang	0:ba0a5e86259f	214
yxyang	0:ba0a5e86259f	215	void
yxyang	0:ba0a5e86259f	216	setLEDs (uint16_t a, uint16_t b, uint16_t c, uint16_t d)
yxyang	0:ba0a5e86259f	217	{
yxyang	0:ba0a5e86259f	218	led1 = a;
yxyang	0:ba0a5e86259f	219	led2 = b;
yxyang	0:ba0a5e86259f	220	led3 = c;
yxyang	0:ba0a5e86259f	221	led4 = d;
yxyang	0:ba0a5e86259f	222	}
yxyang	0:ba0a5e86259f	223
yxyang	0:ba0a5e86259f	224	void
yxyang	0:ba0a5e86259f	225	steerImprovedPointTurns (int16_t line_pos)
yxyang	0:ba0a5e86259f	226	{
yxyang	0:ba0a5e86259f	227	line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
yxyang	0:ba0a5e86259f	228
yxyang	0:ba0a5e86259f	229	// Find desired motor voltages based on the controller
yxyang	0:ba0a5e86259f	230	float pwm_outer = SPEED_PWM * (1.0F
yxyang	0:ba0a5e86259f	231	+ TURN_SENS_OUTER * abs (line_pos) * 2.0F
yxyang	0:ba0a5e86259f	232	/ (float)TSL1401CL_PIXEL_COUNT);
yxyang	0:ba0a5e86259f	233	float pwm_inner = SPEED_PWM * (1.0F
yxyang	0:ba0a5e86259f	234	- TURN_SENS_INNER * abs (line_pos) * 2.0F
yxyang	0:ba0a5e86259f	235	/ (float)TSL1401CL_PIXEL_COUNT);
yxyang	0:ba0a5e86259f	236
yxyang	0:ba0a5e86259f	237	// Write to the appropriate PWM pins
yxyang	0:ba0a5e86259f	238	if (line_pos < 0)
yxyang	0:ba0a5e86259f	239	{
yxyang	0:ba0a5e86259f	240	m1_fwd.write (pwm_outer);
yxyang	0:ba0a5e86259f	241	m1_back.write (0);
yxyang	0:ba0a5e86259f	242	if (pwm_inner >= 0)
yxyang	0:ba0a5e86259f	243	{
yxyang	0:ba0a5e86259f	244	m2_fwd.write (pwm_inner);
yxyang	0:ba0a5e86259f	245	m2_back.write (0);
yxyang	0:ba0a5e86259f	246	}
yxyang	0:ba0a5e86259f	247	else
yxyang	0:ba0a5e86259f	248	{
yxyang	0:ba0a5e86259f	249	m2_fwd.write (0);
yxyang	0:ba0a5e86259f	250	m2_back.write (pwm_inner * -1.0F);
yxyang	0:ba0a5e86259f	251	}
yxyang	0:ba0a5e86259f	252	}
yxyang	0:ba0a5e86259f	253
yxyang	0:ba0a5e86259f	254	if (line_pos >= 0)
yxyang	0:ba0a5e86259f	255	{
yxyang	0:ba0a5e86259f	256	m2_fwd.write (pwm_outer);
yxyang	0:ba0a5e86259f	257	m2_back.write (0);
yxyang	0:ba0a5e86259f	258	if (pwm_inner >= 0)
yxyang	0:ba0a5e86259f	259	{
yxyang	0:ba0a5e86259f	260	m1_fwd.write (pwm_inner);
yxyang	0:ba0a5e86259f	261	m1_back.write (0);
yxyang	0:ba0a5e86259f	262	}
yxyang	0:ba0a5e86259f	263	else
yxyang	0:ba0a5e86259f	264	{
yxyang	0:ba0a5e86259f	265	m1_fwd.write (0);
yxyang	0:ba0a5e86259f	266	m1_back.write (pwm_inner * -1.0F);
yxyang	0:ba0a5e86259f	267	}
yxyang	0:ba0a5e86259f	268	}
yxyang	0:ba0a5e86259f	269	}
yxyang	0:ba0a5e86259f	270
yxyang	0:ba0a5e86259f	271	void
yxyang	0:ba0a5e86259f	272	steerImprovedPointTurnsBackward (int16_t line_pos)
yxyang	0:ba0a5e86259f	273	{
yxyang	0:ba0a5e86259f	274	line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
yxyang	0:ba0a5e86259f	275
yxyang	0:ba0a5e86259f	276	// Find desired motor voltages based on the controller
yxyang	0:ba0a5e86259f	277	float pwm_outer = SPEED_PWM * (1.0F
yxyang	0:ba0a5e86259f	278	+ TURN_SENS_OUTER * abs (line_pos) * 2.0F
yxyang	0:ba0a5e86259f	279	/ (float)TSL1401CL_PIXEL_COUNT);
yxyang	0:ba0a5e86259f	280	float pwm_inner = SPEED_PWM * (1.0F
yxyang	0:ba0a5e86259f	281	- TURN_SENS_INNER * abs (line_pos) * 2.0F
yxyang	0:ba0a5e86259f	282	/ (float)TSL1401CL_PIXEL_COUNT);
yxyang	0:ba0a5e86259f	283
yxyang	0:ba0a5e86259f	284	// Write to the appropriate PWM pins
yxyang	0:ba0a5e86259f	285	if (line_pos < 0)
yxyang	0:ba0a5e86259f	286	{
yxyang	0:ba0a5e86259f	287	m1_back.write (pwm_outer);
yxyang	0:ba0a5e86259f	288	m1_fwd.write (0);
yxyang	0:ba0a5e86259f	289	if (pwm_inner >= 0)
yxyang	0:ba0a5e86259f	290	{
yxyang	0:ba0a5e86259f	291	m2_back.write (pwm_inner);
yxyang	0:ba0a5e86259f	292	m2_fwd.write (0);
yxyang	0:ba0a5e86259f	293	}
yxyang	0:ba0a5e86259f	294	else
yxyang	0:ba0a5e86259f	295	{
yxyang	0:ba0a5e86259f	296	m2_back.write (0);
yxyang	0:ba0a5e86259f	297	m2_fwd.write (pwm_inner * -1.0F);
yxyang	0:ba0a5e86259f	298	}
yxyang	0:ba0a5e86259f	299	}
yxyang	0:ba0a5e86259f	300
yxyang	0:ba0a5e86259f	301	if (line_pos >= 0)
yxyang	0:ba0a5e86259f	302	{
yxyang	0:ba0a5e86259f	303	m2_back.write (pwm_outer);
yxyang	0:ba0a5e86259f	304	m2_fwd.write (0);
yxyang	0:ba0a5e86259f	305	if (pwm_inner >= 0)
yxyang	0:ba0a5e86259f	306	{
yxyang	0:ba0a5e86259f	307	m1_back.write (pwm_inner);
yxyang	0:ba0a5e86259f	308	m1_fwd.write (0);
yxyang	0:ba0a5e86259f	309	}
yxyang	0:ba0a5e86259f	310	else
yxyang	0:ba0a5e86259f	311	{
yxyang	0:ba0a5e86259f	312	m1_back.write (0);
yxyang	0:ba0a5e86259f	313	m1_fwd.write (pwm_inner * -1.0F);
yxyang	0:ba0a5e86259f	314	}
yxyang	0:ba0a5e86259f	315	}
yxyang	0:ba0a5e86259f	316	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	30 May 2017
Imports:	0
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	3
Followers:	1

main.cpp@0:ba0a5e86259f, 2017-05-30 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning