XbeeReceive - Program for receiving XBee packets and controllin…

Users » kunaljacy » Code » XbeeReceive

Kunal Mahajan / Mbed 2 deprecated XbeeReceive

Program for receiving XBee packets and controlling the MoboRobo.

main.cpp@0:04e850e5bb86, 2012-05-05 (annotated)

Committer:: kunaljacy
Date:: Sat May 05 04:07:13 2012 +0000
Revision:: 0:04e850e5bb86

Who changed what in which revision?

User	Revision	Line number	New contents of line
kunaljacy	0:04e850e5bb86	1	#include "mbed.h"
kunaljacy	0:04e850e5bb86	2	#include "MODSERIAL.h"
kunaljacy	0:04e850e5bb86	3
kunaljacy	0:04e850e5bb86	4
kunaljacy	0:04e850e5bb86	5	// Serial Communication with XBee, iRobot, and PC
kunaljacy	0:04e850e5bb86	6	MODSERIAL xbee_receive(p9, p10); // tx, rx
kunaljacy	0:04e850e5bb86	7	Serial irobot(p13, p14); // tx, rx
kunaljacy	0:04e850e5bb86	8	Serial pc(USBTX, USBRX);
kunaljacy	0:04e850e5bb86	9
kunaljacy	0:04e850e5bb86	10	// LEDs for wireless communication status
kunaljacy	0:04e850e5bb86	11	DigitalOut l1(LED1);
kunaljacy	0:04e850e5bb86	12	DigitalOut l2(LED2);
kunaljacy	0:04e850e5bb86	13
kunaljacy	0:04e850e5bb86	14	// PWM channels for arm motor speed control
kunaljacy	0:04e850e5bb86	15	PwmOut m1s(p21); // shoulder
kunaljacy	0:04e850e5bb86	16	PwmOut m2s(p22); // elbow
kunaljacy	0:04e850e5bb86	17	PwmOut m3s(p23); // gripper
kunaljacy	0:04e850e5bb86	18	PwmOut m4s(p24); // wrist
kunaljacy	0:04e850e5bb86	19
kunaljacy	0:04e850e5bb86	20	// Digial channels for arm motor direction control
kunaljacy	0:04e850e5bb86	21	DigitalOut m1d1(p19); // shoulder
kunaljacy	0:04e850e5bb86	22	DigitalOut m1d2(p20);
kunaljacy	0:04e850e5bb86	23	DigitalOut m2d1(p27); // elbow
kunaljacy	0:04e850e5bb86	24	DigitalOut m2d2(p28);
kunaljacy	0:04e850e5bb86	25	DigitalOut m3d1(p29); // gripper
kunaljacy	0:04e850e5bb86	26	DigitalOut m3d2(p30);
kunaljacy	0:04e850e5bb86	27	DigitalOut m4d1(p25); // wrist
kunaljacy	0:04e850e5bb86	28	DigitalOut m4d2(p26);
kunaljacy	0:04e850e5bb86	29
kunaljacy	0:04e850e5bb86	30	// Create Command
kunaljacy	0:04e850e5bb86	31	const char Start = 128;
kunaljacy	0:04e850e5bb86	32	const char FullMode = 132;
kunaljacy	0:04e850e5bb86	33	const char DriveDirect = 145; // 4: [Right Hi] [Right Low] [Left Hi] [Left Low]
kunaljacy	0:04e850e5bb86	34	const char SensorStream = 148;
kunaljacy	0:04e850e5bb86	35	const char PlaySong = 141;
kunaljacy	0:04e850e5bb86	36	const char Song = 140;
kunaljacy	0:04e850e5bb86	37	const char BumpsandDrops = 7;
kunaljacy	0:04e850e5bb86	38
kunaljacy	0:04e850e5bb86	39	/* Global variables with sensor packet info */
kunaljacy	0:04e850e5bb86	40	char Sensor_byte_count = 0;
kunaljacy	0:04e850e5bb86	41	char Sensor_Data_Byte = 0;
kunaljacy	0:04e850e5bb86	42	char Sensor_ID = 0;
kunaljacy	0:04e850e5bb86	43	char Sensor_Num_Bytes = 0;
kunaljacy	0:04e850e5bb86	44	char Sensor_Checksum = 0;
kunaljacy	0:04e850e5bb86	45	short mode;
kunaljacy	0:04e850e5bb86	46
kunaljacy	0:04e850e5bb86	47	void start();
kunaljacy	0:04e850e5bb86	48	void stop();
kunaljacy	0:04e850e5bb86	49	void autonomous();
kunaljacy	0:04e850e5bb86	50	void receive_sensor();
kunaljacy	0:04e850e5bb86	51	void findBomb();
kunaljacy	0:04e850e5bb86	52	void diffuseBomb();
kunaljacy	0:04e850e5bb86	53	void returnHome();
kunaljacy	0:04e850e5bb86	54	void turn_right();
kunaljacy	0:04e850e5bb86	55	void turn_left();
kunaljacy	0:04e850e5bb86	56	void returnHome();
kunaljacy	0:04e850e5bb86	57	void playsong();
kunaljacy	0:04e850e5bb86	58
kunaljacy	0:04e850e5bb86	59	typedef struct {
kunaljacy	0:04e850e5bb86	60	short xAxis;
kunaljacy	0:04e850e5bb86	61	short yAxis;
kunaljacy	0:04e850e5bb86	62	short grip;
kunaljacy	0:04e850e5bb86	63	short left_velocity;
kunaljacy	0:04e850e5bb86	64	short right_velocity;
kunaljacy	0:04e850e5bb86	65	short wrist;
kunaljacy	0:04e850e5bb86	66	short elbow;
kunaljacy	0:04e850e5bb86	67	short mode;
kunaljacy	0:04e850e5bb86	68	} xbee_packet;
kunaljacy	0:04e850e5bb86	69
kunaljacy	0:04e850e5bb86	70	xbee_packet packet;
kunaljacy	0:04e850e5bb86	71	char packetFound = 0;
kunaljacy	0:04e850e5bb86	72
kunaljacy	0:04e850e5bb86	73	void receive_xbee_packet() {
kunaljacy	0:04e850e5bb86	74	char tmp;
kunaljacy	0:04e850e5bb86	75	packetFound = 0;
kunaljacy	0:04e850e5bb86	76	char cs = 0;
kunaljacy	0:04e850e5bb86	77	char data[16];
kunaljacy	0:04e850e5bb86	78	l1 = 0;
kunaljacy	0:04e850e5bb86	79	l2 = 0;
kunaljacy	0:04e850e5bb86	80	if (xbee_receive.rxBufferGetCount() < 20) {
kunaljacy	0:04e850e5bb86	81	l1 = 1;
kunaljacy	0:04e850e5bb86	82	return;
kunaljacy	0:04e850e5bb86	83	}
kunaljacy	0:04e850e5bb86	84	while (xbee_receive.rxBufferGetCount() > 19) {
kunaljacy	0:04e850e5bb86	85	tmp = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	86	if (tmp == 0xF1) {
kunaljacy	0:04e850e5bb86	87	tmp = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	88	if (tmp == 0xE2) {
kunaljacy	0:04e850e5bb86	89	packetFound = 1;
kunaljacy	0:04e850e5bb86	90	break;
kunaljacy	0:04e850e5bb86	91	}
kunaljacy	0:04e850e5bb86	92	}
kunaljacy	0:04e850e5bb86	93	}
kunaljacy	0:04e850e5bb86	94	if (packetFound == 1) {
kunaljacy	0:04e850e5bb86	95	l2 = 1;
kunaljacy	0:04e850e5bb86	96	tmp = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	97	cs = tmp;
kunaljacy	0:04e850e5bb86	98	data[0] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	99	cs ^= data[0];
kunaljacy	0:04e850e5bb86	100	data[1] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	101	cs ^= data[1];
kunaljacy	0:04e850e5bb86	102	data[2] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	103	cs ^= data[2];
kunaljacy	0:04e850e5bb86	104	data[3] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	105	cs ^= data[3];
kunaljacy	0:04e850e5bb86	106	data[4] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	107	cs ^= data[4];
kunaljacy	0:04e850e5bb86	108	data[5] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	109	cs ^= data[5];
kunaljacy	0:04e850e5bb86	110	data[6] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	111	cs ^= data[6];
kunaljacy	0:04e850e5bb86	112	data[7] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	113	cs ^= data[7];
kunaljacy	0:04e850e5bb86	114	data[8] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	115	cs ^= data[8];
kunaljacy	0:04e850e5bb86	116	data[9] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	117	cs ^= data[9];
kunaljacy	0:04e850e5bb86	118	data[10] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	119	cs ^= data[10];
kunaljacy	0:04e850e5bb86	120	data[11] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	121	cs ^= data[11];
kunaljacy	0:04e850e5bb86	122	data[12] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	123	cs ^= data[12];
kunaljacy	0:04e850e5bb86	124	data[13] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	125	cs ^= data[13];
kunaljacy	0:04e850e5bb86	126	data[14] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	127	cs ^= data[14];
kunaljacy	0:04e850e5bb86	128	data[15] = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	129	cs ^= data[15];
kunaljacy	0:04e850e5bb86	130	tmp = xbee_receive.getc();
kunaljacy	0:04e850e5bb86	131	if (cs == tmp) {
kunaljacy	0:04e850e5bb86	132	memcpy(&packet, data, 16);
kunaljacy	0:04e850e5bb86	133	}
kunaljacy	0:04e850e5bb86	134	}
kunaljacy	0:04e850e5bb86	135	}
kunaljacy	0:04e850e5bb86	136
kunaljacy	0:04e850e5bb86	137	void receive_sensor() {
kunaljacy	0:04e850e5bb86	138	char start_character;
kunaljacy	0:04e850e5bb86	139	Sensor_Data_Byte = 0;
kunaljacy	0:04e850e5bb86	140	// Loop just in case more than one character is in UART's receive FIFO buffer
kunaljacy	0:04e850e5bb86	141	while (irobot.readable()) {
kunaljacy	0:04e850e5bb86	142	switch (Sensor_byte_count) {
kunaljacy	0:04e850e5bb86	143	// Wait for Sensor Data Packet Header of 19
kunaljacy	0:04e850e5bb86	144	case 0: {
kunaljacy	0:04e850e5bb86	145	start_character = irobot.getc();
kunaljacy	0:04e850e5bb86	146	if (start_character == 19) Sensor_byte_count++;
kunaljacy	0:04e850e5bb86	147	break;
kunaljacy	0:04e850e5bb86	148	}
kunaljacy	0:04e850e5bb86	149	// Number of Packet Bytes
kunaljacy	0:04e850e5bb86	150	case 1: {
kunaljacy	0:04e850e5bb86	151	Sensor_Num_Bytes = irobot.getc();
kunaljacy	0:04e850e5bb86	152	Sensor_byte_count++;
kunaljacy	0:04e850e5bb86	153	break;
kunaljacy	0:04e850e5bb86	154	}
kunaljacy	0:04e850e5bb86	155	// Sensor ID of next data value
kunaljacy	0:04e850e5bb86	156	case 2: {
kunaljacy	0:04e850e5bb86	157	Sensor_ID = irobot.getc();
kunaljacy	0:04e850e5bb86	158	Sensor_byte_count++;
kunaljacy	0:04e850e5bb86	159	break;
kunaljacy	0:04e850e5bb86	160	}
kunaljacy	0:04e850e5bb86	161	// Sensor data value
kunaljacy	0:04e850e5bb86	162	case 3: {
kunaljacy	0:04e850e5bb86	163	Sensor_Data_Byte = irobot.getc();
kunaljacy	0:04e850e5bb86	164	irobot.getc();
kunaljacy	0:04e850e5bb86	165	Sensor_byte_count = 0;
kunaljacy	0:04e850e5bb86	166	return;
kunaljacy	0:04e850e5bb86	167	}
kunaljacy	0:04e850e5bb86	168	// Read Checksum and update LEDs with sensor data
kunaljacy	0:04e850e5bb86	169	case 4: {
kunaljacy	0:04e850e5bb86	170	Sensor_Checksum = irobot.getc();
kunaljacy	0:04e850e5bb86	171	// Could add code here to check the checksum and ignore a bad data packet
kunaljacy	0:04e850e5bb86	172	Sensor_byte_count = 0;
kunaljacy	0:04e850e5bb86	173	break;
kunaljacy	0:04e850e5bb86	174	}
kunaljacy	0:04e850e5bb86	175	}
kunaljacy	0:04e850e5bb86	176	}
kunaljacy	0:04e850e5bb86	177	return;
kunaljacy	0:04e850e5bb86	178	}
kunaljacy	0:04e850e5bb86	179
kunaljacy	0:04e850e5bb86	180	void actuate() {
kunaljacy	0:04e850e5bb86	181	short right = packet.right_velocity;
kunaljacy	0:04e850e5bb86	182	short left = packet.left_velocity;
kunaljacy	0:04e850e5bb86	183	short xAxis = packet.xAxis;
kunaljacy	0:04e850e5bb86	184	short yAxis = packet.yAxis;
kunaljacy	0:04e850e5bb86	185	short grip = packet.grip;
kunaljacy	0:04e850e5bb86	186	short wrist = packet.wrist;
kunaljacy	0:04e850e5bb86	187	short elbow = packet.elbow;
kunaljacy	0:04e850e5bb86	188
kunaljacy	0:04e850e5bb86	189	if (right \|\| left){
kunaljacy	0:04e850e5bb86	190	irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
kunaljacy	0:04e850e5bb86	191	return;
kunaljacy	0:04e850e5bb86	192	}
kunaljacy	0:04e850e5bb86	193	stop();
kunaljacy	0:04e850e5bb86	194	if (xAxis < 0) {
kunaljacy	0:04e850e5bb86	195	irobot.printf("%c%c%c%c%c", DriveDirect, char(((-xAxis * 5) >> 8) & 0xFF), char(-xAxis * 5 & 0xFF), char(((xAxis * 5) >> 8) & 0xFF), char((xAxis * 5) & 0xFF));
kunaljacy	0:04e850e5bb86	196	return;
kunaljacy	0:04e850e5bb86	197	} else if (xAxis > 0) {
kunaljacy	0:04e850e5bb86	198	irobot.printf("%c%c%c%c%c", DriveDirect, char(((-xAxis * 5) >> 8) & 0xFF), char(-xAxis * 5 & 0xFF), char(((xAxis * 5) >> 8) & 0xFF), char((xAxis * 5) & 0xFF));
kunaljacy	0:04e850e5bb86	199	return;
kunaljacy	0:04e850e5bb86	200	}
kunaljacy	0:04e850e5bb86	201	stop();
kunaljacy	0:04e850e5bb86	202	if (yAxis < 0) {
kunaljacy	0:04e850e5bb86	203	m1d1 = 1;
kunaljacy	0:04e850e5bb86	204	m1d2 = 0;
kunaljacy	0:04e850e5bb86	205	m1s.write((float) (-yAxis)/50);
kunaljacy	0:04e850e5bb86	206	} else if (yAxis > 0) {
kunaljacy	0:04e850e5bb86	207	m1d1 = 0;
kunaljacy	0:04e850e5bb86	208	m1d2 = 1;
kunaljacy	0:04e850e5bb86	209	m1s.write((float) yAxis/50);
kunaljacy	0:04e850e5bb86	210	} else {
kunaljacy	0:04e850e5bb86	211	m1d1 = 0;
kunaljacy	0:04e850e5bb86	212	m1d2 = 0;
kunaljacy	0:04e850e5bb86	213	m1s = 0;
kunaljacy	0:04e850e5bb86	214	}
kunaljacy	0:04e850e5bb86	215	if (elbow < 0) {
kunaljacy	0:04e850e5bb86	216	// pc.printf("elbow\r\n");
kunaljacy	0:04e850e5bb86	217	m2d1 = 1;
kunaljacy	0:04e850e5bb86	218	m2d2 = 0;
kunaljacy	0:04e850e5bb86	219	m2s.write((float) (-elbow)/50);
kunaljacy	0:04e850e5bb86	220	} else if (elbow > 0) {
kunaljacy	0:04e850e5bb86	221	m2d1 = 0;
kunaljacy	0:04e850e5bb86	222	m2d2 = 1;
kunaljacy	0:04e850e5bb86	223	m2s.write((float) elbow/50);
kunaljacy	0:04e850e5bb86	224	} else {
kunaljacy	0:04e850e5bb86	225	m2d1 = 0;
kunaljacy	0:04e850e5bb86	226	m2d2 = 0;
kunaljacy	0:04e850e5bb86	227	m2s.write(0);
kunaljacy	0:04e850e5bb86	228	}
kunaljacy	0:04e850e5bb86	229	if (wrist < 0) {
kunaljacy	0:04e850e5bb86	230	m4d1 = 1;
kunaljacy	0:04e850e5bb86	231	m4d2 = 0;
kunaljacy	0:04e850e5bb86	232	m4s.write((float) 0.2);
kunaljacy	0:04e850e5bb86	233	} else if (wrist > 0) {
kunaljacy	0:04e850e5bb86	234	// pc.printf("wrist\r\n");
kunaljacy	0:04e850e5bb86	235	m4d1 = 0;
kunaljacy	0:04e850e5bb86	236	m4d2 = 1;
kunaljacy	0:04e850e5bb86	237	m4s.write((float) 0.2);
kunaljacy	0:04e850e5bb86	238	} else {
kunaljacy	0:04e850e5bb86	239	m4d1 = 0;
kunaljacy	0:04e850e5bb86	240	m4d2 = 0;
kunaljacy	0:04e850e5bb86	241	m4s.write(0);
kunaljacy	0:04e850e5bb86	242	}
kunaljacy	0:04e850e5bb86	243	if (grip == -1) {
kunaljacy	0:04e850e5bb86	244	// pc.printf("gripper\r\n");
kunaljacy	0:04e850e5bb86	245	m3d1 = 1;
kunaljacy	0:04e850e5bb86	246	m3d2 = 0;
kunaljacy	0:04e850e5bb86	247	m3s.write((float) 0.2);
kunaljacy	0:04e850e5bb86	248	} else if (grip == 1) {
kunaljacy	0:04e850e5bb86	249	m3d1 = 0;
kunaljacy	0:04e850e5bb86	250	m3d2 = 1;
kunaljacy	0:04e850e5bb86	251	m3s.write((float) 0.2);
kunaljacy	0:04e850e5bb86	252	} else {
kunaljacy	0:04e850e5bb86	253	m3d1 = 0;
kunaljacy	0:04e850e5bb86	254	m3d2 = 0;
kunaljacy	0:04e850e5bb86	255	m3s.write(0);
kunaljacy	0:04e850e5bb86	256	}
kunaljacy	0:04e850e5bb86	257	}
kunaljacy	0:04e850e5bb86	258
kunaljacy	0:04e850e5bb86	259	int main() {
kunaljacy	0:04e850e5bb86	260	int count = 0;
kunaljacy	0:04e850e5bb86	261	wait(5); // wait for Create to power up to accept serial commands
kunaljacy	0:04e850e5bb86	262	irobot.baud(57600); // set baud rate for Create factory default
kunaljacy	0:04e850e5bb86	263	start();
kunaljacy	0:04e850e5bb86	264	wait(.5);
kunaljacy	0:04e850e5bb86	265	while (1) {
kunaljacy	0:04e850e5bb86	266	receive_xbee_packet();
kunaljacy	0:04e850e5bb86	267	mode = packet.mode;
kunaljacy	0:04e850e5bb86	268	if (packetFound == 1 && !mode) {
kunaljacy	0:04e850e5bb86	269	//pc.printf("X: %d\tY: %d\tF: %d\tL: %d\tR: %d\tE: %d\tW: %d\r\n", packet.xAxis, packet.yAxis, packet.grip, packet.left_velocity, packet.right_velocity, packet.elbow, packet.wrist);
kunaljacy	0:04e850e5bb86	270	actuate();
kunaljacy	0:04e850e5bb86	271	} else if (packetFound && mode && !count) {
kunaljacy	0:04e850e5bb86	272	autonomous();
kunaljacy	0:04e850e5bb86	273	}
kunaljacy	0:04e850e5bb86	274	else if (packetFound && mode && count) {
kunaljacy	0:04e850e5bb86	275	stop();
kunaljacy	0:04e850e5bb86	276	}
kunaljacy	0:04e850e5bb86	277	}
kunaljacy	0:04e850e5bb86	278	}
kunaljacy	0:04e850e5bb86	279
kunaljacy	0:04e850e5bb86	280	// SEMI-AUTONOMOUS CODE GOES HERE
kunaljacy	0:04e850e5bb86	281	void autonomous(){
kunaljacy	0:04e850e5bb86	282
kunaljacy	0:04e850e5bb86	283	char bnd;
kunaljacy	0:04e850e5bb86	284
kunaljacy	0:04e850e5bb86	285	stop();
kunaljacy	0:04e850e5bb86	286	/*
kunaljacy	0:04e850e5bb86	287	findBomb();
kunaljacy	0:04e850e5bb86	288	diffuseBomb();
kunaljacy	0:04e850e5bb86	289	findBomb();
kunaljacy	0:04e850e5bb86	290	diffuseBomb();
kunaljacy	0:04e850e5bb86	291	returnHome();
kunaljacy	0:04e850e5bb86	292
kunaljacy	0:04e850e5bb86	293	//pc.printf("BND: %X\r\n", Sensor_Data_Byte);
kunaljacy	0:04e850e5bb86	294
kunaljacy	0:04e850e5bb86	295	wait(0.5);
kunaljacy	0:04e850e5bb86	296	*/
kunaljacy	0:04e850e5bb86	297
kunaljacy	0:04e850e5bb86	298	irobot.printf("%c%c", 136, char(1));
kunaljacy	0:04e850e5bb86	299	while(1){
kunaljacy	0:04e850e5bb86	300	receive_xbee_packet();
kunaljacy	0:04e850e5bb86	301	mode = packet.mode;
kunaljacy	0:04e850e5bb86	302	if (!mode){
kunaljacy	0:04e850e5bb86	303	start();
kunaljacy	0:04e850e5bb86	304	return;
kunaljacy	0:04e850e5bb86	305	}
kunaljacy	0:04e850e5bb86	306	}
kunaljacy	0:04e850e5bb86	307	}
kunaljacy	0:04e850e5bb86	308
kunaljacy	0:04e850e5bb86	309	void start() {
kunaljacy	0:04e850e5bb86	310	irobot.putc(Start);
kunaljacy	0:04e850e5bb86	311	irobot.putc(FullMode);
kunaljacy	0:04e850e5bb86	312	wait(.5);
kunaljacy	0:04e850e5bb86	313	}
kunaljacy	0:04e850e5bb86	314
kunaljacy	0:04e850e5bb86	315	void stop() {
kunaljacy	0:04e850e5bb86	316	irobot.printf("%c%c%c%c%c", DriveDirect, char(0), char(0), char(0), char(0));
kunaljacy	0:04e850e5bb86	317	}
kunaljacy	0:04e850e5bb86	318
kunaljacy	0:04e850e5bb86	319	// Reverse - reverse drive motors
kunaljacy	0:04e850e5bb86	320	void reverse() {
kunaljacy	0:04e850e5bb86	321	int speed_right = 200;
kunaljacy	0:04e850e5bb86	322	int speed_left = 200;
kunaljacy	0:04e850e5bb86	323	irobot.putc(DriveDirect);
kunaljacy	0:04e850e5bb86	324	irobot.putc(char(((-speed_right)>>8)&0xFF));
kunaljacy	0:04e850e5bb86	325	irobot.putc(char((-speed_right)&0xFF));
kunaljacy	0:04e850e5bb86	326	irobot.putc(char(((-speed_left)>>8)&0xFF));
kunaljacy	0:04e850e5bb86	327	irobot.putc(char((-speed_left)&0xFF));
kunaljacy	0:04e850e5bb86	328	wait(.85);
kunaljacy	0:04e850e5bb86	329	}
kunaljacy	0:04e850e5bb86	330
kunaljacy	0:04e850e5bb86	331	// Left - drive motors set to rotate to left
kunaljacy	0:04e850e5bb86	332	void turn_left() {
kunaljacy	0:04e850e5bb86	333	int speed_right = 200;
kunaljacy	0:04e850e5bb86	334	int speed_left = 200;
kunaljacy	0:04e850e5bb86	335	irobot.putc(DriveDirect);
kunaljacy	0:04e850e5bb86	336	irobot.putc(char(((speed_right)>>8)&0xFF));
kunaljacy	0:04e850e5bb86	337	irobot.putc(char((speed_right)&0xFF));
kunaljacy	0:04e850e5bb86	338	irobot.putc(char(((-speed_left)>>8)&0xFF));
kunaljacy	0:04e850e5bb86	339	irobot.putc(char((-speed_left)&0xFF));
kunaljacy	0:04e850e5bb86	340	wait(.85);
kunaljacy	0:04e850e5bb86	341	}
kunaljacy	0:04e850e5bb86	342
kunaljacy	0:04e850e5bb86	343	// Right - drive motors set to rotate to right
kunaljacy	0:04e850e5bb86	344	void turn_right() {
kunaljacy	0:04e850e5bb86	345	int speed_right = 200;
kunaljacy	0:04e850e5bb86	346	int speed_left = 200;
kunaljacy	0:04e850e5bb86	347	irobot.putc(DriveDirect);
kunaljacy	0:04e850e5bb86	348	irobot.putc(char(((-speed_right)>>8)&0xFF));
kunaljacy	0:04e850e5bb86	349	irobot.putc(char((-speed_right)&0xFF));
kunaljacy	0:04e850e5bb86	350	irobot.putc(char(((speed_left)>>8)&0xFF));
kunaljacy	0:04e850e5bb86	351	irobot.putc(char((speed_left)&0xFF));
kunaljacy	0:04e850e5bb86	352	wait(.85);
kunaljacy	0:04e850e5bb86	353	}
kunaljacy	0:04e850e5bb86	354
kunaljacy	0:04e850e5bb86	355	// Play Song - define and play a song
kunaljacy	0:04e850e5bb86	356	void playsong() { // Send out notes & duration to define song and then play song
kunaljacy	0:04e850e5bb86	357
kunaljacy	0:04e850e5bb86	358	irobot.putc(Song);
kunaljacy	0:04e850e5bb86	359	irobot.putc(char(0));
kunaljacy	0:04e850e5bb86	360	irobot.putc(char(2));
kunaljacy	0:04e850e5bb86	361	irobot.putc(char(64));
kunaljacy	0:04e850e5bb86	362	irobot.putc(char(24));
kunaljacy	0:04e850e5bb86	363	irobot.putc(char(36));
kunaljacy	0:04e850e5bb86	364	irobot.putc(char(36));
kunaljacy	0:04e850e5bb86	365	wait(0.2);
kunaljacy	0:04e850e5bb86	366	irobot.putc(PlaySong);
kunaljacy	0:04e850e5bb86	367	irobot.putc(char(0));
kunaljacy	0:04e850e5bb86	368	}
kunaljacy	0:04e850e5bb86	369
kunaljacy	0:04e850e5bb86	370	void findBomb(){
kunaljacy	0:04e850e5bb86	371	int right = 400;
kunaljacy	0:04e850e5bb86	372	int left = 400;
kunaljacy	0:04e850e5bb86	373	int c = 0;
kunaljacy	0:04e850e5bb86	374	Sensor_Data_Byte = 0;
kunaljacy	0:04e850e5bb86	375	printf("before while loop: %X\r\n", Sensor_Data_Byte);
kunaljacy	0:04e850e5bb86	376	while((Sensor_Data_Byte != 3) && (Sensor_Data_Byte != 2) && (Sensor_Data_Byte != 1)){
kunaljacy	0:04e850e5bb86	377	irobot.putc(SensorStream);
kunaljacy	0:04e850e5bb86	378	irobot.putc(1);
kunaljacy	0:04e850e5bb86	379	irobot.putc(BumpsandDrops);
kunaljacy	0:04e850e5bb86	380	receive_sensor();
kunaljacy	0:04e850e5bb86	381	printf("bnd: %X\r\n", Sensor_Data_Byte);
kunaljacy	0:04e850e5bb86	382	irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
kunaljacy	0:04e850e5bb86	383	if (!c) {
kunaljacy	0:04e850e5bb86	384	wait(1);
kunaljacy	0:04e850e5bb86	385	}
kunaljacy	0:04e850e5bb86	386	c = 1;
kunaljacy	0:04e850e5bb86	387	}
kunaljacy	0:04e850e5bb86	388	Sensor_Data_Byte = 0;
kunaljacy	0:04e850e5bb86	389	playsong();
kunaljacy	0:04e850e5bb86	390	pc.printf("bomb found\r\n");
kunaljacy	0:04e850e5bb86	391	}
kunaljacy	0:04e850e5bb86	392
kunaljacy	0:04e850e5bb86	393	void diffuseBomb(){
kunaljacy	0:04e850e5bb86	394	// diffuse the bomb --- motor values here
kunaljacy	0:04e850e5bb86	395	//pc.printf("X: %d\tY: %d\tF: %d\tL: %d\tR: %d\tE: %d\tW: %d\r\n", packet.xAxis, packet.yAxis, packet.grip, packet.left_velocity, packet.right_velocity, packet.elbow, packet.wrist);
kunaljacy	0:04e850e5bb86	396	Sensor_Data_Byte = 0;
kunaljacy	0:04e850e5bb86	397	int right;
kunaljacy	0:04e850e5bb86	398	int left;
kunaljacy	0:04e850e5bb86	399	pc.printf("diffusing bomb\r\n");
kunaljacy	0:04e850e5bb86	400	stop();
kunaljacy	0:04e850e5bb86	401	wait(10);
kunaljacy	0:04e850e5bb86	402	right = -100;
kunaljacy	0:04e850e5bb86	403	left = -100;
kunaljacy	0:04e850e5bb86	404	irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
kunaljacy	0:04e850e5bb86	405	wait(0.25);
kunaljacy	0:04e850e5bb86	406	stop();
kunaljacy	0:04e850e5bb86	407	pc.printf("bomb diffused, turning right\r\n");
kunaljacy	0:04e850e5bb86	408	// turn right
kunaljacy	0:04e850e5bb86	409	turn_right();
kunaljacy	0:04e850e5bb86	410	pc.printf("turned right, going home/diffusing next bomb\r\n");
kunaljacy	0:04e850e5bb86	411	Sensor_Data_Byte = 0;
kunaljacy	0:04e850e5bb86	412	}
kunaljacy	0:04e850e5bb86	413
kunaljacy	0:04e850e5bb86	414	void returnHome(){
kunaljacy	0:04e850e5bb86	415	// move the robot for some time
kunaljacy	0:04e850e5bb86	416	int right = 200;
kunaljacy	0:04e850e5bb86	417	int left = 200;
kunaljacy	0:04e850e5bb86	418	irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
kunaljacy	0:04e850e5bb86	419	wait(8);
kunaljacy	0:04e850e5bb86	420	// turn right
kunaljacy	0:04e850e5bb86	421	turn_right();
kunaljacy	0:04e850e5bb86	422	// move the robot for some time
kunaljacy	0:04e850e5bb86	423	irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
kunaljacy	0:04e850e5bb86	424	wait(3);
kunaljacy	0:04e850e5bb86	425	stop();
kunaljacy	0:04e850e5bb86	426	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	05 May 2012
Imports:	15
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	2
Followers:	4

main.cpp@0:04e850e5bb86, 2012-05-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning