Bee - | Mbed

Users » gsulc » Code » Bee

main.cpp@0:178a07cd3e39, 2011-12-16 (annotated)

Committer:: gsulc
Date:: Fri Dec 16 03:45:52 2011 +0000
Revision:: 0:178a07cd3e39

Who changed what in which revision?

User	Revision	Line number	New contents of line
gsulc	0:178a07cd3e39	1	#include "mbed.h"
gsulc	0:178a07cd3e39	2	#include "bees.h"
gsulc	0:178a07cd3e39	3
gsulc	0:178a07cd3e39	4	#define DEBUG 1
gsulc	0:178a07cd3e39	5	#define V_PER_IN 0.0098 // mV/in
gsulc	0:178a07cd3e39	6
gsulc	0:178a07cd3e39	7	#ifdef DEBUG
gsulc	0:178a07cd3e39	8	Serial pc(USBTX, USBRX); // tx, rx
gsulc	0:178a07cd3e39	9	#endif
gsulc	0:178a07cd3e39	10
gsulc	0:178a07cd3e39	11	DigitalOut myled(LED1);
gsulc	0:178a07cd3e39	12
gsulc	0:178a07cd3e39	13	AnalogIn srf(p20);
gsulc	0:178a07cd3e39	14	Serial xbee(p9, p10);
gsulc	0:178a07cd3e39	15	DigitalOut xbee_reset(p11);
gsulc	0:178a07cd3e39	16
gsulc	0:178a07cd3e39	17
gsulc	0:178a07cd3e39	18	///
gsulc	0:178a07cd3e39	19	AnalogIn r_encoder(p15);
gsulc	0:178a07cd3e39	20	AnalogIn l_encoder(p16);
gsulc	0:178a07cd3e39	21
gsulc	0:178a07cd3e39	22	DigitalOut r_charge(p5);
gsulc	0:178a07cd3e39	23	DigitalOut l_charge(p6);
gsulc	0:178a07cd3e39	24
gsulc	0:178a07cd3e39	25	Motor r_motor(p21,p30,p29);
gsulc	0:178a07cd3e39	26	Motor l_motor(p22,p7,p8);
gsulc	0:178a07cd3e39	27
gsulc	0:178a07cd3e39	28	volatile int prev_r_enc = 0;
gsulc	0:178a07cd3e39	29	volatile int prev_l_enc = 0;
gsulc	0:178a07cd3e39	30
gsulc	0:178a07cd3e39	31	volatile int r_tics = 0;
gsulc	0:178a07cd3e39	32	volatile int l_tics = 0;
gsulc	0:178a07cd3e39	33
gsulc	0:178a07cd3e39	34	volatile int r_goal = 0;
gsulc	0:178a07cd3e39	35	volatile int l_goal = 0;
gsulc	0:178a07cd3e39	36
gsulc	0:178a07cd3e39	37	volatile float r_C = -1;
gsulc	0:178a07cd3e39	38	volatile float l_C = -1;
gsulc	0:178a07cd3e39	39
gsulc	0:178a07cd3e39	40	Ticker charge_period;
gsulc	0:178a07cd3e39	41	Ticker discharge_period;
gsulc	0:178a07cd3e39	42
gsulc	0:178a07cd3e39	43	int directions[2*DIRECTIONS+1];
gsulc	0:178a07cd3e39	44
gsulc	0:178a07cd3e39	45	MovementState move_state = Stopped;
gsulc	0:178a07cd3e39	46	///
gsulc	0:178a07cd3e39	47	//DoType dotype;
gsulc	0:178a07cd3e39	48
gsulc	0:178a07cd3e39	49
gsulc	0:178a07cd3e39	50	int main() {
gsulc	0:178a07cd3e39	51	Bee_Type type;
gsulc	0:178a07cd3e39	52	Operation_Mode mode;
gsulc	0:178a07cd3e39	53	float distance;
gsulc	0:178a07cd3e39	54
gsulc	0:178a07cd3e39	55	QTIsensor_init();
gsulc	0:178a07cd3e39	56	charge_period.attach(&QTIsensor_charge,0.02);
gsulc	0:178a07cd3e39	57	wait_ms(0.5);
gsulc	0:178a07cd3e39	58	discharge_period.attach(&QTIsensor_discharge,0.02);
gsulc	0:178a07cd3e39	59
gsulc	0:178a07cd3e39	60	// init
gsulc	0:178a07cd3e39	61	xbee_reset = 0;
gsulc	0:178a07cd3e39	62	wait_ms(1);
gsulc	0:178a07cd3e39	63	xbee_reset = 1;
gsulc	0:178a07cd3e39	64	// set the type of robot
gsulc	0:178a07cd3e39	65	type = Leader;
gsulc	0:178a07cd3e39	66	if(type == Leader)
gsulc	0:178a07cd3e39	67	mode = Blaze;
gsulc	0:178a07cd3e39	68	else if (type == Follower)
gsulc	0:178a07cd3e39	69	mode = GetDirections;
gsulc	0:178a07cd3e39	70
gsulc	0:178a07cd3e39	71	//calibrate(0.5, 40);
gsulc	0:178a07cd3e39	72	//move(100);
gsulc	0:178a07cd3e39	73
gsulc	0:178a07cd3e39	74	//wait(2);
gsulc	0:178a07cd3e39	75	//move_state = Moving;
gsulc	0:178a07cd3e39	76
gsulc	0:178a07cd3e39	77	//move(0.5, 80);
gsulc	0:178a07cd3e39	78
gsulc	0:178a07cd3e39	79	distance = 10000 * srf;
gsulc	0:178a07cd3e39	80	srand((int)distance);
gsulc	0:178a07cd3e39	81
gsulc	0:178a07cd3e39	82	while(1) {
gsulc	0:178a07cd3e39	83	if(DEBUG) {
gsulc	0:178a07cd3e39	84	pc.printf("Left : %d Goal: %d\n\r", l_tics, l_goal);
gsulc	0:178a07cd3e39	85	pc.printf("Right: %d Goal: %d\n\r", r_tics, r_goal);
gsulc	0:178a07cd3e39	86	}
gsulc	0:178a07cd3e39	87
gsulc	0:178a07cd3e39	88	//wait(0.2);
gsulc	0:178a07cd3e39	89	distance = (254 - 6) * srf;
gsulc	0:178a07cd3e39	90
gsulc	0:178a07cd3e39	91	if (DEBUG)
gsulc	0:178a07cd3e39	92	pc.printf("Range: %f\n\r", distance);
gsulc	0:178a07cd3e39	93
gsulc	0:178a07cd3e39	94	if(distance <= 8)
gsulc	0:178a07cd3e39	95	rotate(1);
gsulc	0:178a07cd3e39	96
gsulc	0:178a07cd3e39	97	//distance = srf / V_PER_IN;
gsulc	0:178a07cd3e39	98	//if(DEBUG)
gsulc	0:178a07cd3e39	99	// pc.printf("Distance: %f in\n\r", distance);
gsulc	0:178a07cd3e39	100
gsulc	0:178a07cd3e39	101	//if(xbee.readable())
gsulc	0:178a07cd3e39	102	// directions[1] = xbee.getc();
gsulc	0:178a07cd3e39	103
gsulc	0:178a07cd3e39	104	/*
gsulc	0:178a07cd3e39	105	This state machine swithces between the operating
gsulc	0:178a07cd3e39	106	modes of the robot and calls the appropriate
gsulc	0:178a07cd3e39	107	function in bees.cpp
gsulc	0:178a07cd3e39	108	*/
gsulc	0:178a07cd3e39	109
gsulc	0:178a07cd3e39	110	//calibrate(0.5,50);
gsulc	0:178a07cd3e39	111
gsulc	0:178a07cd3e39	112	/*switch(mode) {
gsulc	0:178a07cd3e39	113	case Seek:
gsulc	0:178a07cd3e39	114	if(DEBUG)
gsulc	0:178a07cd3e39	115	pc.printf("Seeking Pollen\n\r");
gsulc	0:178a07cd3e39	116	if(find_pollen())
gsulc	0:178a07cd3e39	117	mode = Return;
gsulc	0:178a07cd3e39	118	break;
gsulc	0:178a07cd3e39	119	case Return:
gsulc	0:178a07cd3e39	120	if(DEBUG)
gsulc	0:178a07cd3e39	121	pc.printf("Returning to Base\n\r");
gsulc	0:178a07cd3e39	122	if(return_home()) {
gsulc	0:178a07cd3e39	123	if(type == Seeker)
gsulc	0:178a07cd3e39	124	mode = SendDirections;
gsulc	0:178a07cd3e39	125	else if(type == Gatherer)
gsulc	0:178a07cd3e39	126	mode = Gather;
gsulc	0:178a07cd3e39	127	}
gsulc	0:178a07cd3e39	128	break;
gsulc	0:178a07cd3e39	129	case SendDirections:
gsulc	0:178a07cd3e39	130	if(DEBUG)
gsulc	0:178a07cd3e39	131	pc.printf("Sending Directions\n\r");
gsulc	0:178a07cd3e39	132	if(send_directions())
gsulc	0:178a07cd3e39	133	mode = Seek;
gsulc	0:178a07cd3e39	134	break;
gsulc	0:178a07cd3e39	135	case GetDirections:
gsulc	0:178a07cd3e39	136	if(DEBUG)
gsulc	0:178a07cd3e39	137	pc.printf("Waiting for Directions\n\r");
gsulc	0:178a07cd3e39	138	if(get_directions())
gsulc	0:178a07cd3e39	139	mode = Gather;
gsulc	0:178a07cd3e39	140	break;
gsulc	0:178a07cd3e39	141	case Gather:
gsulc	0:178a07cd3e39	142	if(DEBUG)
gsulc	0:178a07cd3e39	143	pc.printf("Gathering Pollen\n\r");
gsulc	0:178a07cd3e39	144	if(goto_pollen())
gsulc	0:178a07cd3e39	145	mode = Gather;
gsulc	0:178a07cd3e39	146	break;
gsulc	0:178a07cd3e39	147	default:
gsulc	0:178a07cd3e39	148	if(DEBUG)
gsulc	0:178a07cd3e39	149	pc.printf("Broken :(\n\r");
gsulc	0:178a07cd3e39	150	}*/
gsulc	0:178a07cd3e39	151
gsulc	0:178a07cd3e39	152	switch(mode) {
gsulc	0:178a07cd3e39	153	case Blaze:
gsulc	0:178a07cd3e39	154	if(DEBUG)
gsulc	0:178a07cd3e39	155	pc.printf("Creating Path\n\r");
gsulc	0:178a07cd3e39	156	blaze();
gsulc	0:178a07cd3e39	157	//pc.printf("now exiting blaze\n\r");
gsulc	0:178a07cd3e39	158	mode = Copy;
gsulc	0:178a07cd3e39	159	break;
gsulc	0:178a07cd3e39	160	case SendDirections:
gsulc	0:178a07cd3e39	161	if(DEBUG)
gsulc	0:178a07cd3e39	162	pc.printf("Sending Directions\n\r");
gsulc	0:178a07cd3e39	163	send_directions();
gsulc	0:178a07cd3e39	164	break;
gsulc	0:178a07cd3e39	165	case GetDirections:
gsulc	0:178a07cd3e39	166	if(DEBUG)
gsulc	0:178a07cd3e39	167	pc.printf("Waiting for Directions\n\r");
gsulc	0:178a07cd3e39	168	if(get_directions())
gsulc	0:178a07cd3e39	169	mode = Copy;
gsulc	0:178a07cd3e39	170	break;
gsulc	0:178a07cd3e39	171	case Copy:
gsulc	0:178a07cd3e39	172	if(DEBUG)
gsulc	0:178a07cd3e39	173	pc.printf("Moving\n\r");
gsulc	0:178a07cd3e39	174	if(copy()) {
gsulc	0:178a07cd3e39	175	if(type == Leader)
gsulc	0:178a07cd3e39	176	mode = SendDirections;
gsulc	0:178a07cd3e39	177	}
gsulc	0:178a07cd3e39	178	break;
gsulc	0:178a07cd3e39	179	default:
gsulc	0:178a07cd3e39	180	if(DEBUG)
gsulc	0:178a07cd3e39	181	pc.printf("Broken :(\n\r");
gsulc	0:178a07cd3e39	182	}
gsulc	0:178a07cd3e39	183	//pc.printf("now checking move_state\n\r");
gsulc	0:178a07cd3e39	184	switch(move_state) {
gsulc	0:178a07cd3e39	185	case Stopped:
gsulc	0:178a07cd3e39	186	//do nothing
gsulc	0:178a07cd3e39	187	r_motor.speed(NEUTRAL);
gsulc	0:178a07cd3e39	188	l_motor.speed(NEUTRAL);
gsulc	0:178a07cd3e39	189	break;
gsulc	0:178a07cd3e39	190	/*case Stop:
gsulc	0:178a07cd3e39	191	//stop the motors
gsulc	0:178a07cd3e39	192	r_motor.stop();
gsulc	0:178a07cd3e39	193	l_motor.stop();
gsulc	0:178a07cd3e39	194	mov_state = Stopped;*/
gsulc	0:178a07cd3e39	195	case Moving:
gsulc	0:178a07cd3e39	196	//sample encoders
gsulc	0:178a07cd3e39	197	//update_movement(0.5);
gsulc	0:178a07cd3e39	198	//pc.printf("now checking encoders right = %d goal = %d, left = %d goal = %d\n\r",r_tics,r_goal,l_tics,l_goal);
gsulc	0:178a07cd3e39	199	if ((r_tics >= r_goal) \|\| (l_tics >= l_goal)) {
gsulc	0:178a07cd3e39	200	r_motor.speed(NEUTRAL);
gsulc	0:178a07cd3e39	201	l_motor.speed(NEUTRAL);
gsulc	0:178a07cd3e39	202	move_state = Stopped;
gsulc	0:178a07cd3e39	203	}
gsulc	0:178a07cd3e39	204
gsulc	0:178a07cd3e39	205	break;
gsulc	0:178a07cd3e39	206	}
gsulc	0:178a07cd3e39	207
gsulc	0:178a07cd3e39	208	myled = 1;
gsulc	0:178a07cd3e39	209	wait(0.2);
gsulc	0:178a07cd3e39	210	myled = 0;
gsulc	0:178a07cd3e39	211	wait(0.2);
gsulc	0:178a07cd3e39	212	}
gsulc	0:178a07cd3e39	213	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	16 Dec 2011
Imports:	21
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@0:178a07cd3e39, 2011-12-16 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning