/*---------------------------------------------------------------+
| Baby Robot three						 |
|                                                                |
| This program is to control the baby bot III   		 |
|								 |
| Author:  Jim Brown                                             |
|                                                                |
| Revision History:                                              |
| 03/04/97 Created                                               |
| 05/02/97 Updated for 3 servos and leds and speaker		 |
|                                                                |
| Copyright (c) 1997 Jim Brown                                   |
| Licensed under the GNU GPL Version 2 or later                  |
|                                                                |
+---------------------------------------------------------------*/

/*

This program works by setting up two timers.  One timer is for
timing the 50ms window of the servo.  The other timer is for
timing the 1-2ms pulse sent to the timer.  So the servos are
pulsed in a round robin fashion.

The first timer interrupts every 20ms/8 (every 2.500ms).  So the first
servo is serviced on the 1st of each 8th interrupt, (0, 8, 16, 24...) 
the next servo is service on the second of 16 interrupts (1, 9, 17,
25...) and so forth for the rest of the servos.

The second timer is for servicing the 1-2ms pulse and only runs
to service the pulse for the current servo and is then stopped.

to set a servo position call the set_servo_position function with
the two parameters being the servo number (0-7) and the position
to move to (0-1000).

*/

#include <reg51.h>

unsigned char servo[8][2]; /* holds the low and high pulse time values (1ms to 2ms) */
short which_servo = 0;      /* selects one of 8 servos (0 - 8) */
short curr_servo = 0;    /* current servo that is being serviced */
long ticker = 0L;	/* a ticker for timing down something */
long ticker2 = 0L;	/* a ticker for turning the head */
long ticker3 = 0L;	/* a ticker for blinking the tie */
long ticker4 = 0L;	/* a ticker to blink the eyes */
int head_pos = 500;
int head_dir = 20;

sbit servo1 = P1^0;
sbit servo2 = P1^1;
sbit servo3 = P1^2;

sbit bumper = P3^6;
short bumper_hit = 0;

#define EYE_OFF 1
#define EYE_ON 0 

sbit right_eye = P1^5;
sbit left_eye = P1^6;

sbit mouth = P1^7;

sbit bow_tie_r = P3^0;
sbit bow_tie_m = P3^1;
sbit bow_tie_l = P3^2;

short tie_blink = 0;
short tie_light = 0;
short eye_light = 0;

sbit speaker = P3^7;

void set_servo_position(int the_servo, int pos);
void low_music_1(void);
void low_music_2(void);
void high_music_1(void);
void high_music_2(void);
void music3(void);

/* timer 0 jumps here on an interrupt */
/* this function just turns off the pulse to the servo */
void pulse_time(void) interrupt 1 using 2
{

	servo1 = 0; /* turn off pulse */
	servo2 = 0;
	servo3 = 0;
 
	TR0 = 0;	/* stop timer 0 for now */

}

/* timer 1 jumps here on an interrupt */
/* this function selects a servo and begins the pulse to it and starts timer 0 */
void window_interrupt(void) interrupt 3 using 3
{
	TR1 = 0;	/* temporarily turn off timer 1 interrupt */
	TH1 = 246;	/* reload timer 1 registers for interrupt after 2500 us */
	TL1 = 63;	/* made it 63 instead of 59 since I turned it off for a short time */
	TR1 = 1;	/* restart timer 1 interrupt */

	if (ticker > 0L) ticker--;  /* just a ticker for timing down something (like sleep) */
	if (bumper == 0) bumper_hit = 1;
	
	if (ticker2 > 0L) ticker2--;
	else {
		ticker2 = 200;
		head_pos += head_dir;
		if (head_pos > 750) head_dir = -20;
		if (head_pos < 250) head_dir = 20;
	}

	if (ticker3 > 0L) ticker3--;
	else {
		ticker3 = 100;
		if (tie_blink) {
			switch(tie_light++)
			{
			case 0: 
				bow_tie_r = EYE_ON; 
				bow_tie_m = bow_tie_l = EYE_OFF; 
				break;
			case 1: 
				bow_tie_m = EYE_ON; 
				bow_tie_r = bow_tie_l = EYE_OFF; 
				break;
			case 2: 
				bow_tie_l = EYE_ON; 
				bow_tie_r = bow_tie_m = EYE_OFF; 
				break;
			case 3: 
				bow_tie_r = bow_tie_m = bow_tie_l = EYE_ON;
				break;
			default:
				bow_tie_r = bow_tie_m = bow_tie_l = EYE_OFF;
				tie_light = 0; 
				break;
			}
		} else {
			bow_tie_r = bow_tie_l = bow_tie_m = EYE_ON;
		}
	}

	if (ticker4 > 0L) ticker4--;
	else {
		ticker4 = 100;
		switch(eye_light++)
			{
			case 0:
				right_eye = EYE_ON;
				left_eye = EYE_OFF;
 				mouth = EYE_ON;
				break; 
			case 2:
				left_eye = EYE_ON;
				right_eye = EYE_OFF;
				break;
			case 1:
			case 3:
			case 5:
			case 6:
				right_eye = left_eye = EYE_ON; 
				break;
			case 4: 
				right_eye = left_eye = EYE_OFF; 
				break;
			default:
				mouth = EYE_OFF;
				eye_light = 0; 
				break;
			}
		
	}

	if (which_servo < 3) /* don't bother if it hasn't been turned on */
	{
		TH0 = servo[which_servo][0];	/* reload timer 0 regs for interrupt after pulse time */
		TL0 = servo[which_servo][1];
		switch(which_servo)
		{
		case 0: servo1 = 1; break;
		case 1: servo2 = 1; break;
		case 2: servo3 = 1; break;
		/* P1 |= (1 << which_servo);	figure the bit to turn on */		
		}
		TR0 = 1;		/* start timer 0 going */
		curr_servo = which_servo;
	}

 	if (++which_servo >= 8) which_servo = 0;
}

/*-------------------------------------------------+
| set up timer 1 & timer 0 for 16 bit overflow     |
| and interrupt every 50ms / 16                    |
| (or every 2.500 ms)                              |
|                                                  |
| so if it overflows at 65535 then subtract        |
| 2500 from it to get 63035                        |
| That means we need to load 63035/256 into th1    |
| and 63035 mod 256 into tl1 (or 246 & 59)         |
+-------------------------------------------------*/
void init(void)
{
	int i;

	/* set all servos to zero */
	for (i = 0; i<8; i++) 
	{
		servo[i][0] = 0;
		servo[i][1] = 0;
	}

	P1 = 0x00;	/* turn port 1 off */
	P3 = 0x00;
	bumper = 1;	/* allow bumper to do reads */
 
	TH1 = 246;	/* setup the timer 1 counter registers */
	TL1 = 59;

	TMOD = 0x11;	/* set timer 0 for 16 bit overflow  & time 1 for 16 bit overflow */
	TR1 = 1;	/* start the timer 1 going (but not timer 0) */
			/* overflow will occur every 20ms */
	IE = 0x8a;	/* make the overflow cause an interrupt! */
			/* 80H means to allow interrupts */
			/* 08H means to enable timer 1 interrupts */
			/* 02H means to enable timer 0 interrupts (but it won't yet!) */
}

/* servo is a number between 0 & 7 */
/* pos is a number between 0 & 1000 */
void set_servo_position(int the_servo, int pos)
{
	unsigned int j;

	/* if the position is out of range stop servicing the servo */
	if (pos > 1000 || pos < 0) 
	{
		servo[the_servo][0] = 0;  /* signal to stop servicing servo */
		servo[the_servo][1] = 0;
	} else {
		j = (unsigned int)65535 - (pos + 1000); /* figure out the pulse time to use */
		servo[the_servo][0] = (unsigned char)(j >> 8); /* store the pulse time */
		servo[the_servo][1] = (unsigned char)(j & 256);
	}
}


/* sleep a given number of hundredths of seconds */
void sleep(int hundredths)
{
	ticker = 1000000L/2500L/250L*hundredths; /* set the timer for a number of seconds */
	while (ticker>0L){}	/* do nothing until timer ticks away to zero */
}

void left_foot_up(void)
{
	set_servo_position(1,750);
	set_servo_position(2,head_pos);
}

void right_foot_up(void)
{
	set_servo_position(1,100);
	set_servo_position(2,head_pos);
}

void left_foot_forward(void)
{
	set_servo_position(0,0);
	set_servo_position(2,head_pos);
}

void right_foot_forward(void)
{
	set_servo_position(0,600);
	set_servo_position(2,head_pos);
}

void center_feet(void)
{
	set_servo_position(2,500);
	set_servo_position(1,500);
	set_servo_position(0,500);
}

void two_step_forward(void)
{
	left_foot_up();
	sleep(50);
	left_foot_forward();
	if (bumper_hit) return;
	sleep(150);
	right_foot_up();
	sleep(50);
	right_foot_forward();
	sleep(150);
}


void two_step_backward(void)
{
	left_foot_up();
	sleep(100);
	right_foot_forward();
	sleep(250);
	right_foot_up();
	sleep(100);
	left_foot_forward();
	sleep(250);
}

void walk_forward(void)
{
	int n;
	for (n=0; n<5; n++)
	{
		if (bumper_hit) return;
		two_step_forward();
	}
}

void dance(void)
{
	int k;

	tie_blink = 1;
	music3();
	music3();
	center_feet();
	for (k=0; k<5; k++)
	{
		set_servo_position(2,0);
		right_foot_up();
		low_music_1();
		set_servo_position(2,500);
		left_foot_up();
		low_music_2();
	}
	center_feet();
	for (k=0; k<5; k++)
	{
	 	set_servo_position(2, 0);
		high_music_1();
		set_servo_position(2,1000);
		high_music_2();
	}
	for (k=0; k<5; k++)
	{
		set_servo_position(2,500);
		right_foot_up();
		low_music_1();
		set_servo_position(2,1000);
		left_foot_up();
		low_music_2();
	}
	center_feet();
	for (k=0; k<5; k++)
	{
	 	set_servo_position(2, 0);
		high_music_1();
		set_servo_position(2,1000);
		high_music_2();
	}
	center_feet();
	music3();
	left_foot_up();
	music3();
	tie_blink = 0;
}

void walk_backward(void)
{
	int n;
	for (n=0; n<5; n++)
	{
		two_step_backward();
	}
}

void blink_eyes(void)
{
	/* P1.5 is the right eye LED */
	/* P1.6 is the left eye LED */
	right_eye = EYE_OFF;
	sleep(250);
	left_eye = EYE_OFF;
	right_eye = EYE_ON;
	sleep(250);
	left_eye = EYE_ON;
	mouth = EYE_OFF;
	sleep(500);
	mouth = EYE_ON;
	bow_tie_r = EYE_OFF;
	bow_tie_m = EYE_OFF;
	bow_tie_l = EYE_OFF;
	sleep(250);
	bow_tie_r = EYE_ON;
	sleep(250);
	bow_tie_m = EYE_ON;
	sleep(250);
	bow_tie_l = EYE_ON;
	sleep(500);
	set_servo_position(2,1000);
	sleep(500);
	set_servo_position(2,0);
	sleep(500);
	set_servo_position(2,500);	
}

void high_beep(int dur)
{
	int i;

	for (i=0; i<dur; i++)
	{
		speaker = 1;
		sleep(1);
		speaker = 0;
		sleep(1);
	}
}

void low_beep(dur)
{
	int idur;
	int i;

	idur = dur/2;
	for (i=0; i<idur; i++)
	{
		speaker = 1;
		sleep(2);
		speaker = 0;
		sleep(2);
	}
}

void very_low_beep(dur)
{
	int idur;
	int i;

	idur = dur/4;
	for (i=0; i<idur; i++)
	{
		speaker = 1;
		sleep(4);
		speaker = 0;
		sleep(4);
	}
}

void low_music_1(void)
{

	low_beep(40);
	sleep(10);	
	low_beep(40);
	sleep(20);
	very_low_beep(20);
	sleep(5);
	very_low_beep(20);
	sleep(5);
	very_low_beep(20);
	sleep(5);

}
void low_music_2(void)
{
	low_beep(40);
	sleep(10);	
	low_beep(40);
	sleep(20);
	low_beep(10);
	sleep(20);
	very_low_beep(20);
	sleep(5);
	very_low_beep(20);
	sleep(5);
	very_low_beep(20);
	sleep(5);
}

void high_music_1(void)
{
	high_beep(40);
	sleep(10);	
	high_beep(40);
	sleep(20);
	low_beep(20);
	sleep(5);
	low_beep(20);
	sleep(5);
	low_beep(20);
	sleep(5);
}
void high_music_2(void)
{
	high_beep(40);
	sleep(10);	
	high_beep(40);
	sleep(20);
	high_beep(10);
	sleep(20);
	low_beep(20);
	sleep(5);
	low_beep(20);
	sleep(5);
	low_beep(20);
	sleep(5);
}



void music3(void)
{
	int x;

	for (x=0; x<3; x++)
	{
		high_beep(20);
		sleep(20);
		low_beep(20);
		sleep(20);
		very_low_beep(20);
		sleep(20);
	}
}


void main(void)
{
	int t;

	init();	/* init the timer int regs */

	while (1) /* do forever */
	{
		center_feet();
		sleep(250);
		blink_eyes();
		music3();
		high_beep(10);
		bumper_hit = 0;
		for (t=0; t<5; t++)
		{
			if (bumper_hit) break;
			walk_forward();
		}
		if (!bumper_hit) dance();
		while(!bumper_hit) walk_forward();
		music3();
		music3();
		bumper_hit = 0;
		walk_backward();
 		while(!bumper_hit) walk_backward();
		music3();
		music3();
		music3();
		for (t=0; t<10; t++) sleep(500);
	}

}