odc.c

int max_light=255;
int constant_light_number=120; /* This seems to work pretty well for our needs */
int light_detection_process = 0;
int bumper_detection_process = 0;
int ir_detector_process = 0;
int max_sense = 140;



/* 	clip(int val, int min, int max)
	This function allows us to set limits on the values returned
	by certain sensors to give us a better and more accurate reading
	This function was found and rewritten from the library functions
*/
int clip (int val, int min, int max)
{
	if (val < min)
	{
		return min;
	}
	else if (val > max)
	{
		return max;
	}
	else
	{
	    return val;
	}
}

/*	gotolight(int left_photo, int right_photo)
	This function calculates how we should go to the light
*/
int gotolight(int left_photo,int right_photo)

{
	int photo_gain = 1;
	int rotation;
	int diff = left_photo - right_photo;
	rotation  = clip( (photo_gain * diff), - 100, 100);
	return rotation;
}

/*	find_lightest_direction()
	In order to find the light (or our tracking object)
	We need to do this to help the bot know which way to turn and move
	In combination with the other functions this allows us to find with
	relative precision the location of the light and dynamically keep it
	in 'sight'
*/
void find_lightest_direction()
{
	int i,direction=0;
	int turn=-1,test_photo=1;
	int light;
	if(photo(1)>photo(2)) {turn=1; test_photo=2;}
	for(i=0;i<11;i++)
	{
		light=photo(test_photo);
		if(light<max_light)
		{
			max_light=light;
			direction=i;
		}
		drive(0,turn*40);
		wait(0.1);
		stop();
	}
	for(i=0;i<direction;i++)
	{
		drive(0,turn*40);
		wait(0.1);
		stop();
	}
}

/*	track_light() ** DO NOT EVEN TOUCH THIS IT WILL MAKE YOU CRY **
	This function is essential for the project and any attempt to disable this will result in a dead bot that will do nothing but chase you around
	with lazer beams and eat you whole with a cherry on top.
	Seriously...
	This function must be forked to be a processes to enable speed to detect
	light and other obsticles without causing endless madness of bumping into
	every object that the robot cannot find. */

void track_light()
{
  int floor = 0;
  int forward_def=80,rot_def;			
  int left,right;
  int loop = 1;
  printf("4:Goto Light\n");           
  init_motors();
  max_light=255;
  find_lightest_direction();
  while(loop)
    {
      floor = analogport(5);  
      if (floor>=195)
      {
		forward_def=0;
		drive(-50,0);
		wait(0.1);
		stop();
		drive(-20,(photo(0) - photo(1)+40));
		wait(0.1);
		stop();
		max_light=255; 
      }
      else forward_def=20;
      left=photo(1);
      right=photo(2);
      
      if((left <= max_sense) || (right <= max_sense)){
          loop=0;
          drive(2,0);
          stop();
          printf("Stopped");
      } else {
          ir_evade();
	      rot_def=gotolight(left,right);
	      drive(forward_def,rot_def);
	      wait(0.1);
	  }      
    }
}


/*
	bumper_detector() ** MUST BE RUN AS OWN PROCESSS **
	The purpose of this function is to allow the robot to detect any
	obsticles that may cause it to not be able to continue on its
	given path.
*/
void bumper_detector(){
	while(1)
	{
		int bumper_sensors = bumper();
			if (bumper_sensors == 0b0001){
				printf("Front Left");
			  	drive(-30,0);
				wait(0.200000);
		    	drive(0, 40);
			}
			if (bumper_sensors == 0b0010){
				printf("Front Right");
			  	drive(-30,0);
		    	wait(0.200000);
		    	drive(0, -40);
				wait(0.300000);
			}
			if (bumper_sensors == 0b0011){
				printf("Front");
				drive(-30,0);
	  	    	wait(0.200000);
				drive(0, 40);
			}
		/* These three are least obvious as they will be called
			the least during the challanges
		*/
			if (bumper_sensors == 0b0100 || bumper_sensors == 0b1000 || bumper_sensors == 0b1100){
				printf("Reverse!");
				drive(30,0);
		    	wait(0.200000);
		    	drive(0, 40);
				wait(0.300000);
			}
			if (bumper_sensors == 0b0101){
				printf("Left");
				drive(0, 40);
			}
			if (bumper_sensors == 0b1010){
				printf("Right");
				drive(0, -40);
			}
			if (bumper_sensors == 0b1111){
				printf("KERNEL PANIC TO THE RESCUE");
				tone(257.0,100.0);
			}
	} /* End While */
} /* Bumper Safety Processes */

void line_detector()
{
	int ground_detector=0;
	while(1)
	{
	    ground_detector = analogport(5);
		if (ground_detector>=195)
	    {
	    	stop();
	    }
	}
}

/*
	move_straight() ** MUST BE RUN WITH PROCESS FORK OR IT NOT WORK **
	This function allows the robot to create a process that lets it drive
	This process must be broken when the robot detects any of the following:
		* IR Sensors (Dual) (Balance)
		* Light Sensors (Quad) (Balance)
		* Microphone (Single)
	The function does not directly control its activity but is just to keep
	the robot moving
*/
void move_straight()

{
	/* The reason for the drive wait stop combination is to
	 due to a bug in the robots which causes it to not execute any further
	 drive or logic functions until drive() is completed
	*/
		drive(15,0);
		wait(10.0);
		stop();
}

int drive_track_forward = 90;
int drive_track_rotation = 60;
void ir_follow_human()
{
	int ir=0;
	int bumper = 0;
	int zero_bumper = 0; /* Obviously */
	init_motors();
	while(1){
		if(zero_bumper && (!bumper))
		{
			wait(0.5);
		} else if(ir == 0) {
			stop();
		} else if(ir == 4) {
			drive(drive_track_forward,0);
		} else if(ir == 2){
			drive(drive_track_forward,drive_track_rotation);
		} else if(ir == 1){
			drive(drive_track_forward,-drive_track_rotation);
			wait(0.1);
		}
		zero_bumper = bumper;
	}
}
/*	ir_evade()
	This function is used to evade ojects that may come in our way also
	it is used on final approach to main zone (includes lights)
*/
void ir_evade()
{
	int ir_sensor=0;
		if(photo(0) > max_sense || photo(1) > max_sense){
			ir_sensor=ir_detector();
			if(ir_sensor == 4){
				 	drive(-30,0);
			    	wait(0.200000);
			    	drive(0, 50);
					wait(0.520000);
					/* We need to have a stop here for detection */
			}
			if (ir_sensor == 2){
			    	drive(-30,0);
			    	wait(0.200000);
			}
			if (ir_sensor == 1){
				    drive(-30,-10);
				    wait(0.200000);
			}
		}
}

void main()
{
	/* Create a process or a few to run, this will be killed when we detect
	a stop that we need. See above for more information
	*/
	light_detection_process =	start_process(track_light());	
	/*ir_detector_process =			start_process(ir_evade());*/
	/*bumper_detection_process =	start_process(bumper_detector());*/
}