//  alternate functions =============================================================

void 
select_alt_program(void)
{
	#define LAST_TEST   14

	UBYTE  a;
	int selection;

	pause(500);
	CLEAR_DISPLAY 

	selection = 0;
	
	while(RIGHT_BUMPER == 0){
		a = P0 & 0x0f;    //  get position of the selector switch
		if(selection != a){
			selection = a;
			CLEAR_DISPLAY 
			sprintf(string, "%d %s", selection,alt[selection-1]);
			send_string(string); 
		}
	}  // end while loop


	CLEAR_DISPLAY
	sprintf(string, "executing %s", alt[selection-1]);
    send_string(string); 

	switch(selection){
		case(1): distances();
		case(2): cmucam();
		case(3): boomarang();		
		case(4): i2c_compass_test();  //  compass_test();
		case(5): motor_test();
		case(6): rc();
	    case(7): rf_test();
		case(8): sentry();
		case(9): compass_test_3();
		case(10): sound_test();
		case(11): servo_test();
		case(12): ir_calibrate();
		case(13): cap_meter();
		case(14): rc_test();

	}

} // end function

/*------------------------*/
// #1

void 
distances(void)
{
	unsigned int p;
	CLEAR_DISPLAY       
	sprintf(string, "L-IR   Sonar   R-IR\0"); send_string(string); 

	while(1){
		move_cursor(1,2);   //  begining of second line
		p = left_ir( );
		send_decimal_3(p);  //  display it

		move_cursor(8,2);   //  begining of second line
		p = sonar( );       //  get distance
		send_decimal_3(p);  //  display it

		move_cursor(16,2);   //  begining of second line
		p = right_ir( );
		send_decimal_3(p);  //  display it

//		pause(500);
	}

}

/*-------------------------*/
// 2
void cmucam(void)
{
	UBYTE a;

	pause(800);
	CLEAR_DISPLAY

	// SEND SETUP STRINGS
	
//	sprintf(string, "\n\0"); send_string_to_camera(string); 
//  send('F');
//send('J');
//send_to_camera(13);
	
	while(1){
//		a = get_byte_from_camera();
		//send_decimal_3(a);
		if(a == ':'){break;}
	}
	send('A');

	// remove mirror image,  auto gain on 
	//sprintf(string, "cr 18 108\n\0"); send_string_to_camera(string); 
	
	//pause(30);
	
	// auto WB on  (off is 19 32) 
	//sprintf(string, "cr 19 33\n\0"); send_string_to_camera(string); 
	//pause(30);
	
	// low frame rate
	//sprintf(string, "cr 17 9\n\0"); send_string_to_camera(string); 
	//pause(30);
	
	// middle mass mode on  (off is 0)
	//sprintf(string, "mm 1\n\0"); send_string_to_camera(string); 
	//pause(30);
	
	// line mode on  (off is 0)
	//sprintf(string, "lm 1\n\0"); send_string_to_camera(string); 
	//pause(30);
	
	// auto gain off
	//sprintf(string, "cr 19 32\n\0"); send_string_to_camera(string); 
	//pause(30);
	
	// set exposure to 20
	//sprintf(string, "cr 16 20\n\0"); send_string_to_camera(string); 
	//pause(30);
	
	// set to raw mode, no "ACK"
//send_to_camera('r');
//send_to_camera('m');
//send_to_camera('3');
//send_to_camera(13);

	
	
//	sprintf(string, "rm3\n\0"); send_string_to_camera(string); 
	pause(30);
	send('B');

	
	// poll mode on
//	sprintf(string, "pm1\n\0"); send_string_to_camera(string); 
//send_to_camera('p');
//send_to_camera('m');
//send_to_camera('1');
//send_to_camera(13);

pause(30);
	send('C');

	// grab color and report info
//	sprintf(string, "tw\n\0"); send_string_to_camera(string); 
//send_to_camera('t');
//send_to_camera('w');
//send_to_camera(13);

pause(30);
//	send('D');	
	while(1){
//		a = get_byte_from_camera();
		if(a == 'M'){break;}
	}
	send('D');	

	

	while(1); // the end

}

// ------------------------------------
// 3
void boomarang(void)
{
	unsigned int aim, now, L;
	unsigned long int start_time;

    aim = bearing();  // store current bearing

    steer(STRAIGHT, 25);
    motor_speed(50);
    now = bearing();

	start_time = current_time;
 	while( current_time < start_time + 6000){         // go forward 6s
        //if(LEFT_BUMPER OR RIGHT_BUMPER){STOP while(1);}
		now = bearing();
		CLEAR_DISPLAY
		send_decimal_3(now);
        if(now < aim){now += 360;}
        L = now - aim;
        if (L <= 5 || L >= 355) {
		  steer(STRAIGHT, 6);
		}
		else if (L < 180 ){
		  steer(SLIGHTLY_LEFT, 6);
		}
        else {
	      steer(SLIGHTLY_RIGHT, 6);
		}
      }

    aim = aim + 180;        // find opposite direction
    if(aim > 360){aim = aim - 360;} // make a correction if neccessary
 
	CLEAR_DISPLAY
	send_decimal_3(now);
	send(' ');
	send_decimal_3(aim);

	STOP
	while(!LEFT_BUMPER) ;

// time for a U-turn
    motor_speed(40);
	steer(LEFT, 25);
//	now=bearing();
//  while(NOT is_bearing_within_range(now, aim + 5, 10)) {

// keep turning until U-turn is complete 
    pause(2000);              //  
    while(1){                
	    now = bearing();
        CLEAR_DISPLAY
		send('n'); send (' ');
	    send_decimal_3(now);
	    send(' '); send('a'); send(' ');
	    send_decimal_3(aim);
	    send ('\n');
        if(now < aim){now += 360;}
        L = now - aim;
		send('L'); send(' ');
		send_decimal_3(L);
        if(L < 10 ){send('C'); break;}   // quit turning if close enough
        if(L > 320 ){send('T'); break;}  // quit turning if gone too far
	    }
// U-turn is complete, go straight 
    steer(STRAIGHT, 25);
    motor_speed(50);
    start_time = current_time;   // start timing
    while(current_time < start_time + 6000){                        // go forward 6s
        //if(LEFT_BUMPER OR RIGHT_BUMPER){STOP while(1);}
		now = bearing();
		CLEAR_DISPLAY
		send_decimal_3(now);
        if(now < aim){now += 360;}
        L = now - aim;
        if (L <= 5 OR L >= 355) {
		  steer(STRAIGHT, 6);
		}
		else if (L < 180 ){
		  steer(SLIGHTLY_LEFT, 6);
		}
        else {
	      steer(SLIGHTLY_RIGHT, 6);
		}
    }
    STOP 
	while(NOT LEFT_BUMPER);

}

//-----------------------------------

// 4
/*
void compass_test(void)  //  continuously show compass reading.
                         //  motor can be turned on and off using bumper
{
    unsigned int v; 

    while(1){
        v = bearing( );
        CLEAR_DISPLAY
        send_decimal_3(v);

        if (LEFT_BUMPER){
          motor_speed(50);
          while(LEFT_BUMPER);       // wait for user to let go of the bumper
            pause(50);
        }

        if (RIGHT_BUMPER){
          motor_speed(0);
          while(RIGHT_BUMPER);       // wait for user to let go of the bumper
            pause(50);
        }

        pause(200);
    }
}
*/
/*-------------------------*/
	// 5
void motor_test(void){
int speed=0;

STOP
send_decimal_3(speed);
while(LEFT_BUMPER OR RIGHT_BUMPER);

while(1){

    if(LEFT_BUMPER){
        speed = speed + 10; 
        motor_speed(speed);
        pause(50);                // de-bounce delay
	    CLEAR_DISPLAY
        send_decimal_3(speed);
        while(LEFT_BUMPER);       // wait for user to let go of the bumper
        pause(50);
        }

    if(RIGHT_BUMPER){
        speed = speed - 10; 
        motor_speed(speed);
        pause(50);                //  de-bounce delay
        CLEAR_DISPLAY
        send_decimal_3(speed);
        while(RIGHT_BUMPER);       // wait for user to let go of the bumper
        pause(50);
        }
    pause(50);   // why this works I dont know
 
} // end loop
} // end function

//-----------------------------------
// 6
void rc(void)
{
	UBYTE a;
	int b;
	while(1)
	{

		//if(LEFT_BUMPER){STOP}
		//if(RIGHT_BUMPER){STOP} 
		a = rf_getc();
		if(a=='h'){motor_speed(50);}
		else if(a=='v'){veer_left();}
		else if(a=='s' OR a==' '){STOP }
		else if(a=='n'){turn_right(15); steer(STRAIGHT,25);}
		else if(a=='b'){move(-30);}
		else if(a=='.'){rf_putc('.');}     //  echo test
		else if(a=='c'){five_point_scan(); aim_sonar(AHEAD,25);}
		else if(a=='g'){motor_speed(25);}
		else if(a=='a'){next_state= ASSESSMENT; state_controller();}

else if(a=='q'){tone(C3,250);}   
else if(a=='w'){tone(D3,250);}   
else if(a=='e'){tone(E3,250);}   
else if(a=='r'){tone(F3,250);}   
else if(a=='t'){tone(G3,250);}   
else if(a=='y'){tone(A4,250);}   
else if(a=='u'){tone(B4,250);}   
else if(a=='i'){tone(C4,250);}   

		else if(a=='y'){
			b = sonar();
			if(b>70){
				motor_speed(100);
			}
			else{
				STOP; 
		  		pause(1000); 
		  		say(IM_AFRAID_THATS_SOMETHING_I_CANNOT_ALLOW);
				pause(6000);
			}
		}
//		else{tone(A3,500); tone(A2,500);}
		else if(a=='1'){say(HEAD_SHAKE);}
		else if(a=='2'){say(CUCKOO);}
		else if(a=='3'){say(UH_OH);}
		else if(a=='4'){say(WHAT_THE);}
		else if(a=='5'){say(NOW_CUT_THAT_OUT);}
		else if(a=='6'){say(SNORE);}
		else if(a=='7'){say(GIGGLE);}
		else if(a=='8'){say(DOH);}
		else if(a=='9'){say(OH_BROTHER);}
		else if(a=='0'){say(BOING);}
		else if(a=='-'){say(BLEEP);}

		else{say(THAT_DOES_NOT_COMPUTE);}	
   }
  
 // for tv remote control 
  	while(1)
	{
		a = get_rc_code();
		if(a==64){motor_speed(50);}
		else if(a==96){turn_left(20); steer(STRAIGHT,25);}
		else if(a==16){STOP }
		else if(a==80){turn_right(20); steer(STRAIGHT,25);}
		else if(a==112){motor_speed(-50);}
		else if(a==4){
			b = sonar();
			if(b>70){
				motor_speed(100);
			}
			else{
				STOP; 
		  		pause(1000); 
		  		say(IM_AFRAID_THATS_SOMETHING_I_CANNOT_ALLOW);
				pause(6000);
			}
		}
//		else{tone(A3,500); tone(A2,500);}
		else if(a==72){say(HEAD_SHAKE);}
		else if(a==44){say(CUCKOO);}
		else if(a==0){say(UH_OH);}
		else if(a==27){say(WHAT_THE);}
		else if(a==24){say(NOW_CUT_THAT_OUT);}
		else if(a==32){say(SNORE);}

		else{say(THAT_DOES_NOT_COMPUTE);}	
   }
}

//----------------------
/*
void compass_test_2(void){         //  go north
    unsigned int p;

    motor_speed(50);
    while(1){
      pause(200);
      p = bearing();
      if(p < 10 OR p > 290){steer(STRAIGHT,25); break;}
      if(p < 165){steer(LEFT,25); break;}
      steer(RIGHT,25); 
    }
}
*/
/*--------------------------*/

void compass_test_3(void)             //  go north, then south
{
unsigned int p;

motor_speed(50);
aim_sonar(AHEAD,25);
HEAD_NORTH:
  pause(200);

  check_distances();
  if(zone[3]<50){goto HEAD_SOUTH;}
  p = bearing();
  if(p<20 OR p>280){steer(STRAIGHT,25); goto HEAD_NORTH;}
  if(p<165){steer(LEFT,25); goto HEAD_NORTH;}
  steer(RIGHT,25); 
  goto HEAD_NORTH;

HEAD_SOUTH:
  check_distances();
  if(zone[3]<50){goto HEAD_NORTH;}
  p = bearing();
  if(p<175 OR p>155){steer(STRAIGHT,25); goto HEAD_SOUTH;}
  if(p<165){steer(RIGHT,25); goto HEAD_SOUTH;}
  steer(LEFT,25); 
  goto HEAD_SOUTH;

}

/*-----------------------------*/
void i2c_compass_test(void)
{ 
	UBYTE  val, b;

		val = read_compass_register(0);
		send_decimal_3(val);
		send(' ');
		pause(500);
		val = read_compass_register(1);
		send_decimal_3(val);
		send(' ');
		pause(500);
		val = read_compass_register(2);
		send_decimal_3(val);
		send(' ');
		pause(500);
		val = read_compass_register(3);
		send_decimal_3(val);
		send(' ');
		pause(500);

send('\n');

		val = read_compass_register(8);
		send_decimal_3(val);
		send(' ');
		pause(500);
		val = read_compass_register(9);
		send_decimal_3(val);
		send(' ');
		pause(500);
		val = read_compass_register(10);
		send_decimal_3(val);
		send(' ');
		pause(500);
		val = read_compass_register(11);
		send_decimal_3(val);
		send(' ');
		pause(500);
	
	while(1);
}
//-------------------------------------------------
	// 7
void rf_test(void)
{
	unsigned int hz;
	unsigned long ticks, microseconds;

	TMOD &= 0xf0; TMOD |= 0x01;       // set up timer
	send(0xc0);                       // use the second line of the display
	while(1){                         // beginning of main loop
	    send('A'); 
		while (TACH_INPUT == 1);      // if in the middle of a pulse, wait

        send('B'); 
        while (TACH_INPUT == 0);      // wait for pulse to start

		TR0=0; TH0=0; TL0=0;             // clear timer
		TR0=1;                           // start timer

		while (TACH_INPUT == 1);      // wait for end of pulse 
	    send('C'); 

		while (TACH_INPUT == 0);      // wait for beginning of next pulse 
		TR0=0;                           // stop the timer
	    send('D'); 
		
		ticks = TH0;                   // combine the timer bytes
		ticks = (ticks << 8) + TL0;    //
		
		microseconds = (ticks * 13 / 12);
		hz = 1000000/microseconds;

    	send(0x03);	send(0xc0); 
		SPACE SPACE SPACE SPACE SPACE SPACE SPACE SPACE
		SPACE SPACE SPACE SPACE SPACE SPACE SPACE SPACE
        send(0x03); send(0xc0);
        send_decimal_4(hz); SPACE send('H'); send('z'); SPACE
		
	} // end of main loop
} // end of tach

/*---------------------------------*/
// 8
void sentry(void){

int a;
	pause(3000);
	while(1){
		a = sonar();
		if (a<80){
			say (WARNING_WARNING_WARNING);
			pause(10000);
		}
	}
}

//---------------------------------
// 10
void sound_test(void)
{

UINT duration = 500, a;   // each tone is half a second each

pause(1000); 
say(IM_COMPLETELY_OPERATIONAL)	;pause(8000);
say(WARNING_LOW_BATTERY);pause(3000);
say(WHAT_THE);pause(3000);
say(DOH);pause(3000);
say(HA_HA);pause(3000);
say(DOING);pause(3000);					
say(BOING);pause(3000);					
say(WOO_HOO)		;pause(3000);		
say(OH_BROTHER)				;pause(3000);
say(UH_OH)				;pause(3000);
say(BLEEP)					;pause(3000);
say(HEAD_SHAKE)				;pause(3000);
say(YEEHAA)					;pause(4000);
say(SNORE)					;pause(4000);
say(CUCKOO)					;pause(4000);
say(NOW_CUT_THAT_OUT)		;pause(4000);
say(HEY_KNOCK_IT_OFF)		;pause(4000);
say(WHAT_DO_YOU_THINK_YOURE_DOING)	;pause(4000);
say(WARNING_WARNING_WARNING)	;pause(4000);
say(THAT_DOES_NOT_COMPUTE)	;pause(4000);
say(MONEY)					;pause(18000);
say(OH_GOOD_GRIEF)			;pause(3000);
say(THERES_SOMETHING_SCREWY)	;pause(4000);
say(SCREECH)					;pause(4000);
say(AW_CRAP)					;pause(4000);
say(IM_AFRAID_THATS_SOMETHING_I_CANNOT_ALLOW)	;pause(8000);
say(IM_SORRY_DAVE)			;pause(8000);
while(1);


/*		
	while(1){
	a = sonar();
	    if  (a < 50){
			tone(C1,duration);
			tone(D1,duration);
			tone(E1,duration);
			tone(F1,duration);
			tone(G1,duration);
			tone(A2,duration);
			tone(B2,duration);
			tone(C2,duration);
			tone(D2,duration);
			tone(E2,duration);
			tone(F2,duration);
			tone(G2,duration);
			tone(A3,duration);
			tone(B3,duration);
			tone(C3,duration);
			tone(D3,duration);
			tone(E3,duration);
			tone(F3,duration);
			tone(G3,duration);
			tone(A4,duration);
			tone(B4,duration);
			tone(C4,duration);
			tone(D4,duration);
			tone(E4,duration);
			tone(F4,duration);
			tone(G4,duration);
			tone(A5,duration);
			tone(B5,duration);
			tone(C5,duration);
			tone(D5,duration);
			tone(E5,duration);
			tone(F5,duration);
			tone(G5,duration);

			pause(1000);

		}
	}
*/

}
//-----------------------------------------------
//  11
void servo_test(void)  //  use the bumper to control the servo
{
unsigned int n;

n = 1500;
aim_sonar(n,50);
steer(n,50);
send_decimal_4(n);
while(LEFT_BUMPER OR RIGHT_BUMPER);

while(1){

  if(LEFT_BUMPER){
    n = n + 100; 
    aim_sonar(n,6);
	steer(n,6);
    pause(50);                // de-bounce delay
	CLEAR_DISPLAY
    send_decimal_4(n);
    while(LEFT_BUMPER);       // wait for user to let go of the bumper
    pause(50);
  }

  if(RIGHT_BUMPER){
    n = n - 100; 
    aim_sonar(n,6);
    steer(n,6);
	pause(50);                //  de-bounce delay
    CLEAR_DISPLAY
    send_decimal_4(n);
    while(RIGHT_BUMPER);       // wait for user to let go of the bumper
    pause(50);
  }
 pause(50);   // why this works I dont know
 
} // end loop
} // end function
//SS:
//  n=500;  
//  while(n <= 1500)
//  {
 //   HOME //send(0x18);
//    send_decimal_4(n);
//    aim_sonar(n,50);
//    n+=100;
//  }
//  pause(2000);
//  goto SS;
//}

//----------------
// Sonar To Servo Demo
//
//LOOP_S:
//p = sonar( );
//if(p>100){p=100;}
//motor_speed(p);

//if(p<2 || p>300){goto LOOP_S;}
//q=((p-2)*4)+1100;
//aim_sonar(q,20);
//goto LOOP_S;

/*---------------------------------*/
// 12
// --------------------------------


void ir_calibrate(void ) 	// for the Sharp GP2D02 distance sensor

{

// dislays the code from both IR sensors

	unsigned int distance, a, b, c;
	unsigned char count;

	while(1)
	{

        CLEAR_DISPLAY
        distance = 0;
		LEFT_IR_CONTROL = 0;     // start
		pause(2);   		// start delay, 1ms (t1)
		
		while (LEFT_IR_DATA == 0);	// wait while it measures  (t2)
	
		for ( count=1 ; count<=8 ; count++ ){ 	// read 8 bits
			LEFT_IR_CONTROL = 1;
			set_timer(20); while(!TF0);           //  stay high for 20us (200us is max)
			LEFT_IR_CONTROL = 0;                  // request a bit
			set_timer(70); while(!TF0);           // valid data delay (t5)
			distance *= 2;                      	// shift left
			if (LEFT_IR_DATA == 1) { distance++;}
		}
		
		LEFT_IR_CONTROL = 1;
		pause (3);  // intermeasurement delay (t6)

        send_decimal_3(distance);
		SPACE SPACE
		//---
		
		distance = 0;
		RIGHT_IR_CONTROL = 0;     // start
		pause(2);   		// start delay, 1ms (t1)
		
		while (RIGHT_IR_DATA == 0);	// wait while it measures  (t2)

	// read 8 bits		
		for ( count=1 ; count<=8 ; count++ ){ 
			RIGHT_IR_CONTROL = 1;
			set_timer(20); while(!TF0);           //  stay high for 20us (200us is max)
			RIGHT_IR_CONTROL = 0;                  // request a bit
			set_timer(70); while(!TF0);           // valid data delay (t5)
			distance *= 2;                      	// shift left
			if (RIGHT_IR_DATA == 1) { distance++;}
		}
		
		RIGHT_IR_CONTROL = 1;
		pause (3);  // intermeasurement delay (t6)
		
        send_decimal_3(distance);

		pause(300);
	
    }
}

//----------------------------------------------
	// 13
void cap_meter(void){
	unsigned long ticks, microseconds, pf, start_time;

TMOD &= 0xf0; TMOD |= 0x01;       // set up timer

while(1){
CLEAR_DISPLAY
TR0=0; TH0=0; TL0=0; TF0=0;            // clear timer
// ready
while(TACH_INPUT == 0);
while(TACH_INPUT == 1);
//start timer
TR0=1;                           // start timer
while(TACH_INPUT == 0);
while(TACH_INPUT == 1);
//stop timer
TR0=0;                           // stop the timer
ticks = TH0;                   // combine the timer bytes
ticks = (ticks << 8) + TL0;    //
microseconds = (ticks * 13 / 12);
pf = microseconds;
if(pf >=21){pf = pf - 21 ;}
else{pf=0;}
//send('a');
if (TF0 == 0){
//send('b');

	sprintf(string, "%lu pF ", pf);
	send_string(string);
}
else{
//send('c');

	start_time = current_time;
	ticks=0;	
	while(current_time < start_time + 1000){
		while(TACH_INPUT == 0);
		while(TACH_INPUT == 1);
		ticks++;
	}  //  end of while loop

send_decimal_4(1000/ticks);
SPACE
send('n');
send('F');


//	sprintf(string, "%lu nF ", ticks);
//	send_string(string);
}  //  end of else
//send('d');

pause(1000);
tone(A3, 100);
//
}  //  end of main loop
}

//---------------------------------------------

// 14
void rc_test(void)
{
	UBYTE a;
	
	while(1){
		a = get_rc_code();
		send_decimal_3(a);
		pause(1000);
		CLEAR_DISPLAY
	}
}

//---------------