/*
    C code for the Main robot controller.
    This controller sends servo positions to the
    servo bank controllers.

    (C) 2004 justin_sane huntjas2@msu.edu

	Version 1.5
*/

// This is the config register for the options that we need
#pragma config |= 0x3FF0

/* pin definitions */

bit sel_switch  @ PORTB.0;	// attached to the mode controll switch
//bit serial_in   @ PORTB.1;	// (in) serial data in from computer
bit serial_out  @ PORTB.2;	// (out) serial out to computer
bit ir_servo    @ PORTB.3;	// (out) outout for IR sensor
							// portb.4 is PGM, unusable in LVP mode
bit led         @ PORTB.5;	// (out) led 
//bit data        @ PORTB.7;	// (in)  data input form main computer
//bit clock       @ PORTB.6;	// (in)  clock input form main computer

bit r_ready     @ PORTA.0;	// (in)  right bank ready
bit l_ready     @ PORTA.1;	// (in)  left bank ready
bit l_clock     @ PORTA.2;	// (out) left bank
bit r_clock     @ PORTA.3;	// (out) clock out to right bank
							// porta.5 is MCLR, unusable in LVP mode
bit data_out	@ PORTA.4;	// (out) data out is shared by both banks
bit ir_enable	@ PORTA.6;	// (out) start control to ir
bit ir_data		@ PORTA.7;	// (in)  data back from the IR


#define mode_forward 	0
#define mode_turning	1
#define mode_backward	2

unsigned char r_servos[6];		// positions of the servos in the banks
unsigned char l_servos[6];		

unsigned char ir_servo_pos;		// position of the gimble servo
#define  ir_servo_base  118		// base time offset for the gimgle servo
int ir_dir;						// current sweep direction for this servo

unsigned char r_angle; 			// angle of right side
unsigned char l_angle; 			// angle of left side

int forward;					// positive is forward movement
int turn;						// positive is left turn



#include "sine.c"				// sine functions
#include "serial.c"				// serial functions




/* setup the system on boot */
void start_up(void)
{
	TRISB = 0xC3;  // bit 0, 1, 6, 7 input   all others, output
	TRISA = 0x83;  // Bits 2, 3, 4, 5, 6 are outputs 

	CMCON = 0x07;  // comparators off

	// init the ports
	PORTA = 0; 
	PORTB = 0; 

	serial_out = 1; // thid is the rest line state

	new_line();

	send_string("ANT Version 1.5 2004 Jason Hunt");

	new_line();

	ir_servo_pos  = 128;

	l_angle = 0;
	r_angle = 0;

    ir_dir = 1;
}
/* end of start_up */




/* move the ir servo */
void update_ir_servo(void)
{
    ir_servo_pos += ir_dir;

	// swap directions if we are out range
	if ((ir_servo_pos == 128 - 64) || (ir_servo_pos == 128 + 64)) 
		ir_dir = -ir_dir;

	unsigned char pos =  ir_servo_pos;
	unsigned char base = ir_servo_base;


	ir_servo = 1;				// turn on pwm control line

	if (base) while(--base);	// delay for base position
	if (pos) while(--pos);		// dealy for actual position

	ir_servo = 0;				// line off
}
/* end of update ir servo */




/* sends all of the servo position to the banks */
// This can update both banks at the same time
void update_servos(void)
{
	// these work as flags and servo index counters	
	int l_done = 0, r_done = 0, t;

	// wait until both servo banks have been updated
	while (!l_done || !r_done)
    {
		// if left side is ready, start transmittion
		if (l_ready && l_done == 0)
		{
			send_byte(l_bank, 0); // send "bank update" command

			for (t = 0; t < 6; t++)
				send_byte(l_bank, l_servos[t]); // send the next servo position

			l_done = 1; // inticate start of transmittion
		}

		// if left side is ready, start transmittion
		if (r_ready && r_done == 0)
		{
			send_byte(r_bank, 0); // send "bank update" command

			for (t = 0; t < 6; t++)
				send_byte(r_bank, r_servos[t]); // send the next servo position

			r_done = 1; // inticate start of transmittion
		}
	}
}
/* end of update servos */



/* calculate the new servo positions */
void calc_servo_pos(unsigned l_rake, unsigned r_rake, unsigned height)
{
	// calculate trig values for the left side
	unsigned l_000_deg = sine(l_angle,     	 	 height);
	unsigned l_180_deg = sine(l_angle + deg_180, height);
	unsigned l_090_deg = sine(l_angle + deg_90,  l_rake);
	unsigned l_270_deg = sine(l_angle + deg_270, l_rake);

	// trig for right side
	unsigned r_000_deg = sine(r_angle,     		 height);
	unsigned r_180_deg = sine(r_angle + deg_180, height);
	unsigned r_090_deg = sine(r_angle + deg_90,  r_rake);
	unsigned r_270_deg = sine(r_angle + deg_270, r_rake);

	// lifting, right side (larger number, leg farther up)
	r_servos[0] = r_180_deg+30;
	r_servos[2] = r_000_deg;
	r_servos[4] = r_180_deg-30;     // (-10)

	// raking, right side (larger number, more forward)
	r_servos[1] = r_090_deg - 30;  // bring rear legs back farther
	r_servos[3] = r_270_deg - 30;
	r_servos[5] = r_090_deg - 30; 

	// lifting, left side   
	l_servos[0] = l_000_deg + 30;
	l_servos[2] = l_180_deg;
	l_servos[4] = l_000_deg - 30 ;  // (-10)


	// raking, left side (larger number, more forward)
	l_servos[1] = l_270_deg - 30;
	l_servos[3] = l_090_deg - 30;
	l_servos[5] = l_270_deg - 30;
}
/* end of calc servo pos */




/* sends the direction data to the main computer */
void send_direction()
{
/*	if (forward > 0) send_string("F");
	if (forward < 0) send_string("B");

	if (turn > 0) send_serial('L');
	if (turn < 0) send_serial('R');

	if (!forward && !turn) send_serial('S'); // stopped

   	space();*/
}
/* end of send_direction */



/* update the angular positions */
void update_movement(void)
{
	unsigned l_rake = 3; // default rake amplitude divisor
	unsigned r_rake = 3;
	unsigned height = 4; 

	//  movement forward or backwards
	if (forward)
	{
		// move the cycle forward and sync legs
		r_angle += forward;	l_angle = r_angle; 
	
		// forward plus turn 
		if (turn > 0) { l_rake = 5; r_rake = 2;} // left turn
		if (turn < 0) { l_rake = 2; r_rake = 5;}  // right turn
	}
	
	// standing rotation (no movement forward or backwards)
	if (turn && !forward)
	{  			
		l_angle -= turn;	// adjust the angle in the direction we are turning
		r_angle += turn; 
	} 	

	// update the servo positions
	calc_servo_pos(l_rake, r_rake, height);
}
/* end of update movement */




/* get analog data from the ir ranger */
unsigned get_ir_dist(void)
{
	ir_enable = 1; // start charging the compare cap

	unsigned long count = 0;

    while (!ir_data && count < 256) count++; // find out time until compare

	ir_enable = 0; // start discharging

    if (count > 255) count = 255;

    return(count);
}



/* main function */
void main( void)
{
	// send the startup string
	start_up();

    unsigned dist;

    unsigned led_count = 0;

	unsigned long count = 0;


	int mode = mode_forward;
	
	forward = 4;
	turn = 0;	

	while (1)
	{
       //	send_direction();		// send debugging info

		dist = get_ir_dist();

		// send the scan data
		send_hex(ir_servo_pos);
		space();
		send_hex(dist);
    	new_line();

		// switch in forward (run position)
		if (sel_switch)
		{
		 	update_movement();		// calculate all of the new pos data
			update_servos();
		}

		update_ir_servo();


		if (mode == mode_forward && dist > 40)
		{
			mode = mode_backward;
			forward = -4;
			turn = 0;
			count = 0;
		}

		if (mode == mode_backward && count > 50)
        {
			mode = mode_turning;
			turn = 4;
			forward = 0;
			count = 0;
		}

		if (mode == mode_turning && count > 75)
        {
			mode = mode_forward;
			forward = 4;
			turn = 0;
			count = 0;
		}

		count++;


		/* stuff to blink the led */
		dist = 255 - dist;

		if (led_count > dist)
		{
			led = !led;
			led_count = 0;
		}

		if (dist == 0) led = 0;

		led_count += 20;
	} 
}
/* end of main */