Remote Control Car

Georgia Tech ECE 4180 Embedded Systems Design Final Project

Team Members

Raj Madisetti Arjun Sonti

main.cpp@3:4956cc0efdf3, 2020-11-19 (annotated)

Committer:: rmadisetti3
Date:: Thu Nov 19 17:23:54 2020 +0000
Revision:: 3:4956cc0efdf3
Parent:: 2:5c9526ccb055
Child:: 4:29839de66eae

Added Autopilot function

Who changed what in which revision?

User	Revision	Line number	New contents of line
rmadisetti3	0:20a8dfc396cb	1	#include "mbed.h"
rmadisetti3	0:20a8dfc396cb	2	#include "XNucleo53L0A1.h"
rmadisetti3	0:20a8dfc396cb	3	#include "Servo.h"
rmadisetti3	0:20a8dfc396cb	4	#include "motordriver.h"
rmadisetti3	0:20a8dfc396cb	5	#include <stdio.h>
rmadisetti3	1:1c1ad0c1c260	6	Serial pc(USBTX,USBRX);
rmadisetti3	2:5c9526ccb055	7	Serial blue(p9,p10);
rmadisetti3	1:1c1ad0c1c260	8	DigitalOut shdn(p26);
rmadisetti3	0:20a8dfc396cb	9	DigitalOut myled(LED1);
rmadisetti3	0:20a8dfc396cb	10
rmadisetti3	1:1c1ad0c1c260	11	#define VL53L0_I2C_SDA p28
rmadisetti3	1:1c1ad0c1c260	12	#define VL53L0_I2C_SCL p27
rmadisetti3	0:20a8dfc396cb	13
rmadisetti3	3:4956cc0efdf3	14	#define AUTOPILOT 10
rmadisetti3	3:4956cc0efdf3	15	#define FORWARD 1
rmadisetti3	3:4956cc0efdf3	16	#define REVERSE -1
rmadisetti3	3:4956cc0efdf3	17	#define STOP 0
rmadisetti3	3:4956cc0efdf3	18
rmadisetti3	1:1c1ad0c1c260	19	static XNucleo53L0A1 *board=NULL;
rmadisetti3	0:20a8dfc396cb	20	Motor M(p21, p22, p23, 1); // pwm, fwd, rev, can brake
rmadisetti3	1:1c1ad0c1c260	21	Servo S(p24);
rmadisetti3	3:4956cc0efdf3	22	int state = 0; //global variable stop state
rmadisetti3	3:4956cc0efdf3	23	int status;
rmadisetti3	3:4956cc0efdf3	24	uint32_t distanceCenter;
rmadisetti3	3:4956cc0efdf3	25	uint32_t distanceLeft;
rmadisetti3	3:4956cc0efdf3	26	uint32_t distanceRight;
rmadisetti3	3:4956cc0efdf3	27
rmadisetti3	3:4956cc0efdf3	28	void getNewState() {
rmadisetti3	3:4956cc0efdf3	29	//Logic for AdaFruit App
rmadisetti3	3:4956cc0efdf3	30	char bnum=0;
rmadisetti3	3:4956cc0efdf3	31	char bhit=0;
rmadisetti3	3:4956cc0efdf3	32	if (blue.readable()) {
rmadisetti3	3:4956cc0efdf3	33	if (blue.getc()=='!') {
rmadisetti3	3:4956cc0efdf3	34	if (blue.getc()=='B') { //button data packet
rmadisetti3	3:4956cc0efdf3	35	bnum = blue.getc(); //button number
rmadisetti3	3:4956cc0efdf3	36	bhit = blue.getc(); //1=hit, 0=release
rmadisetti3	3:4956cc0efdf3	37	if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
rmadisetti3	3:4956cc0efdf3	38	myled = bnum - '0'; //current button number will appear on LEDs
rmadisetti3	3:4956cc0efdf3	39	switch (bnum) {
rmadisetti3	3:4956cc0efdf3	40	case '1': //AutoPilot Mode
rmadisetti3	3:4956cc0efdf3	41	if (bhit=='1') {
rmadisetti3	3:4956cc0efdf3	42	state = 10; //autopilot state
rmadisetti3	3:4956cc0efdf3	43	}
rmadisetti3	3:4956cc0efdf3	44	break;
rmadisetti3	3:4956cc0efdf3	45	case '5': //forward
rmadisetti3	3:4956cc0efdf3	46	if (bhit=='1') {
rmadisetti3	3:4956cc0efdf3	47	state = 1; //forward state
rmadisetti3	3:4956cc0efdf3	48	} else {
rmadisetti3	3:4956cc0efdf3	49	state = 0; //stop state
rmadisetti3	3:4956cc0efdf3	50	}
rmadisetti3	3:4956cc0efdf3	51	break;
rmadisetti3	3:4956cc0efdf3	52	case '6': //reverse
rmadisetti3	3:4956cc0efdf3	53	if (bhit=='1') {
rmadisetti3	3:4956cc0efdf3	54	state = -1; //reverse state
rmadisetti3	3:4956cc0efdf3	55	} else {
rmadisetti3	3:4956cc0efdf3	56	state = 0; //stop state
rmadisetti3	3:4956cc0efdf3	57	}
rmadisetti3	3:4956cc0efdf3	58	break;
rmadisetti3	3:4956cc0efdf3	59	case '7': //left
rmadisetti3	3:4956cc0efdf3	60	if (bhit=='1') {
rmadisetti3	3:4956cc0efdf3	61	S = S - 0.5; //turn left
rmadisetti3	3:4956cc0efdf3	62	}
rmadisetti3	3:4956cc0efdf3	63	break;
rmadisetti3	3:4956cc0efdf3	64	case '8': //right
rmadisetti3	3:4956cc0efdf3	65	if (bhit=='1') {
rmadisetti3	3:4956cc0efdf3	66	S = S + 0.5; //turn right
rmadisetti3	3:4956cc0efdf3	67	}
rmadisetti3	3:4956cc0efdf3	68	break;
rmadisetti3	3:4956cc0efdf3	69	}
rmadisetti3	3:4956cc0efdf3	70	}
rmadisetti3	3:4956cc0efdf3	71	}
rmadisetti3	3:4956cc0efdf3	72	}
rmadisetti3	3:4956cc0efdf3	73	}
rmadisetti3	3:4956cc0efdf3	74	}
rmadisetti3	3:4956cc0efdf3	75
rmadisetti3	3:4956cc0efdf3	76	void autoPilot() {
rmadisetti3	3:4956cc0efdf3	77	if ((distanceCenter >= 250 \|\| distanceCenter == 0)) {
rmadisetti3	3:4956cc0efdf3	78	M.speed(1.0);
rmadisetti3	3:4956cc0efdf3	79	S = 0.5;
rmadisetti3	3:4956cc0efdf3	80	}
rmadisetti3	3:4956cc0efdf3	81	else if (distanceRight > distanceLeft) { //More space on right so turn right
rmadisetti3	3:4956cc0efdf3	82	S = 1.0;
rmadisetti3	3:4956cc0efdf3	83	}
rmadisetti3	3:4956cc0efdf3	84	else { //or turn left
rmadisetti3	3:4956cc0efdf3	85	S = 0;
rmadisetti3	3:4956cc0efdf3	86	}
rmadisetti3	3:4956cc0efdf3	87	}
rmadisetti3	0:20a8dfc396cb	88
rmadisetti3	0:20a8dfc396cb	89
rmadisetti3	0:20a8dfc396cb	90	int main() {
rmadisetti3	3:4956cc0efdf3	91	//LIDAR Initialization
rmadisetti3	1:1c1ad0c1c260	92	DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
rmadisetti3	1:1c1ad0c1c260	93	/* creates the 53L0A1 expansion board singleton obj */
rmadisetti3	1:1c1ad0c1c260	94	board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
rmadisetti3	1:1c1ad0c1c260	95	shdn = 0; //must reset sensor for an mbed reset to work
rmadisetti3	1:1c1ad0c1c260	96	wait(0.1);
rmadisetti3	1:1c1ad0c1c260	97	shdn = 1;
rmadisetti3	1:1c1ad0c1c260	98	wait(0.1);
rmadisetti3	1:1c1ad0c1c260	99	/* init the 53L0A1 board with default values */
rmadisetti3	1:1c1ad0c1c260	100	status = board->init_board();
rmadisetti3	1:1c1ad0c1c260	101	while (status) {
rmadisetti3	1:1c1ad0c1c260	102	pc.printf("Failed to init board! \r\n");
rmadisetti3	1:1c1ad0c1c260	103	status = board->init_board();
rmadisetti3	1:1c1ad0c1c260	104	}
rmadisetti3	1:1c1ad0c1c260	105
rmadisetti3	3:4956cc0efdf3	106	while(1) { //main loop for motor control
rmadisetti3	3:4956cc0efdf3	107	status = board->sensor_centre->get_distance(&distanceCenter);
rmadisetti3	3:4956cc0efdf3	108	pc.printf("D=%ld mm\r\n", distanceCenter);
rmadisetti3	3:4956cc0efdf3	109	getNewState();
rmadisetti3	3:4956cc0efdf3	110	if ((distanceCenter >= 250 \|\| distanceCenter == 0) && state == FORWARD) {
rmadisetti3	3:4956cc0efdf3	111	M.speed(1.0);
rmadisetti3	3:4956cc0efdf3	112	}
rmadisetti3	3:4956cc0efdf3	113	else if (state == REVERSE) {
rmadisetti3	3:4956cc0efdf3	114	M.speed(-1.0);
rmadisetti3	3:4956cc0efdf3	115	}
rmadisetti3	3:4956cc0efdf3	116	else if (state == AUTOPILOT) {
rmadisetti3	3:4956cc0efdf3	117	autoPilot();
rmadisetti3	3:4956cc0efdf3	118	}
rmadisetti3	3:4956cc0efdf3	119	else {
rmadisetti3	3:4956cc0efdf3	120	M.speed(0);
rmadisetti3	2:5c9526ccb055	121	}
rmadisetti3	2:5c9526ccb055	122	}
rmadisetti3	3:4956cc0efdf3	123
rmadisetti3	0:20a8dfc396cb	124	}