map_bot.c

用于机器人自动低分辨路的地图测绘程序。用于机器人控制测绘。分为远端控制端和本地控制端。控制电机为标准舵机。

		{
			update_timeout = 30;
			pwm_value[0] = 0;
			pwm_value[1] = 0;
			pwm_value[2] = 0;
			pwm_value[3] = 0;
		}


		// Check for recevive error
    	if (OERR)
    	{
    		CREN = 0;  // reset recieve logic
    		CREN = 1;
    	}



		TMR1IF = 0; // clear flag
	} 

	if (TMR2IF)	
	{
//		led_0 = encoder_0;

		if (encoder_0 != last_encoder_0)
		{
			encoder_low[0]++;

			if (encoder_low[0] == 0)
				encoder_high[0]++;


			last_encoder_0 = encoder_0;
			encoder_0_count++;
		}

		if (encoder_1 != last_encoder_1)
		{
			encoder_low[1]++;

			if (encoder_low[1] == 0)
				encoder_high[1]++;

			last_encoder_1 = encoder_1;
			encoder_1_count++;
		}


		if (encoder_2 != last_encoder_2)
		{
			encoder_low[2]++;

			if (encoder_low[2] == 0)
				encoder_high[2]++;


			last_encoder_2 = encoder_2;
			encoder_2_count++;
		}

		if (encoder_3 != last_encoder_3)
		{
			encoder_low[3]++;

			if (encoder_low[3] == 0)
				encoder_high[3]++;

			last_encoder_3 = encoder_3;
			encoder_3_count++;
		}

		pwm_cycle++;

		// Check for cycle starts.
		// Do not set the pin if the value is zero
		if (pwm_cycle == 0 && pwm_value[0]) pwm_out_0 = 1; 
		if (pwm_cycle == 0 && pwm_value[1]) pwm_out_1 = 1;
		if (pwm_cycle == 0 && pwm_value[2]) pwm_out_2 = 1; 
		if (pwm_cycle == 0 && pwm_value[3]) pwm_out_3 = 1;

		// turn outputs off after their duty has passed
		if (pwm_cycle > pwm_value[0]) pwm_out_0 = 0;
		if (pwm_cycle > pwm_value[1]) pwm_out_1 = 0;
		if (pwm_cycle > pwm_value[2]) pwm_out_2 = 0;
		if (pwm_cycle > pwm_value[3]) pwm_out_3 = 0;

	//	TMR2 = 0xe0; // reset timer  this is about 500 hz

		TMR2 = 0xd0; // reset timer  this is about 500 hz

	//	TMR2 = 0x20;

		// See if we are waiting to start the A->D
		// Also, make sure it's not already running
		if (wait_analog && !GO)
		{
			// Reduce wait count
			wait_analog--;

			// Done waiting, start the converter
			if (wait_analog == 0) 
                GO = 1;
		}

		TMR2IF = 0; // clear flag
	}

	// The analog to digital converter is done
    if (ADIF)
    {  
		// Save current values
		analog_low[cur_analog] = ADRESL;
		analog_high[cur_analog] = ADRESH & 0x03;

		// Setup for next analog channel
     	cur_analog++;

		// Wrap arund back to first
    	if (cur_analog >= NUM_ANALOG) cur_analog = 0;

		// Create mask for new channel
    	unsigned char mask = cur_analog << 3;

    	ADCON0 &= 0b.1100.0111;
    	ADCON0 |= mask; // mask off channel bits, and set new channel

		// Setup to wait to fire the go signal
		wait_analog = 200;

		ADIF = 0;		// Clear flag
    }

    int_restore_registers
}
/* end of interrupt service routine */

/**************************
** End of interrupt Code **
**************************/


/***********************
**	Serial functions **
***********************/

/* Set the serial port to  9600 8N1 */
void setup_serial(void)
{
//	SPBRG = 129;   // divisor for 9600 baud
	SPBRG = 64;    // divisor for 19200 baud

    TXSTA = 0b.0010.0100;  // transmitt enable, high speed
	RCSTA = 0b.1001.0000; // serial enable

	char ch = RCREG;  // flush buffer
	ch = RCREG;
	ch = RCREG;
}
/* end of setup serial */




/*******************
** Initialization **
*******************/

/* system startup */
void startup( void )
{
	PORTA = 0;			// clear ports
	PORTB = 0;
	PORTC = 0;
	PORTD = 0;
	PORTE = 0;

	TRISA = 0b.1111.1111;	// all inputs
	TRISB = 0b.0000.0001;	// bit 0 is digital in 2, bit 3 is led1
	TRISC = 0b.1000.0000;	// bit 7 is rx
//	TRISD = 0b.1100.0000;	// D6, D7 radio in, all others digital out

	TRISD = 0b.0000.1111;	// D6, D7 radio in, all others digital out

	TRISE = 0b.0000.0111;	// E0-E2 digital in


//		ADCON0 = 0b.1000.0001 | mask;

	// clear core values before interrupts are turned on

    running_counter = 0;
    recv_counter = 0;


	servo_mode = 0;			// finite state machine for servo control
	cur_servo = 0;			// current servo number
	pwm_cycle = 0;			// state of pwm system

	pwm_value[0] = 0;
	pwm_value[1] = 0;
	pwm_value[2] = 0;
	pwm_value[3] = 0;



	// setup timer 0
    OPTION = 4;				// prescaler divide by 16
	T0CS = 0;				// internel clock
    TMR0 = 0;				// clear the timer
    T0IE = 1;				// enable interrupt for timer 0


	// setup timer 1
	TMR1L = 0;			// reset timer
	TMR1H = 0;

	TMR1ON = 0; 			// shut off timer to modify values

	T1CKPS1 = 1;			// setup prescaler
	T1CKPS0 = 0;
	T1OSCEN = 0;			// shut off oscillator

	TMR1CS = 0;				// internal clock
	TMR1IE = 1;				// enable interrupt

	TMR1ON = 1; 			// turn timer on
	TMR1IF = 0;				// clear interrupt flag

	TMR2 = 0xe0;
	TMR2ON = 1;				// enable timer 2
	TMR2IE = 1;

	servo_pos[0] = 0x80;	// setup inital servo positions
	servo_pos[1] = 0x80;	
	servo_pos[2] = 0x80;
	servo_pos[3] = 0x80;	
	servo_pos[4] = 0x80;	
	servo_pos[5] = 0x80;

   update_timeout = 0;

   reading_data = 0;
   read_timeout = 0;
   cur_recv_byte = 0; // Waiting for first byte
   cur_send_byte = 0; // Waiting for first byte
	last_valid = 1;

   RCIF = 0;	// Clear receive interrupt
   RCIE = 1; 	// Enable receive interrupts

   TXIF = 0;	// Clear receive interrupt
   TXIE = 1; 	// Enable receive interrupts


	// Analog to Digital Setup
    cur_analog = 0;

//	ADCON1 = 0b.1000.0010;	// RE0, 1, 2 : digital RA0,1,2,3,5 : analog

	ADCON1 = 0b.1000.0000;	// RE0, 1, 2 : digital RA0,1,2,3,5 : analog


	ADCS1 = 1;				// Fosc/32 clocl
	ADCS0 = 0;
	ADCON0 &= 0b.1100.0111;
	ADON = 1;					// turn on converter

    ADIF =0;
    ADIE =1;

	GO = 1;
	
    GIE = 1;				// interrupts allowed 
	PEIE = 1;				// enable periphial interrupts

encoder_0_count = 0;
encoder_0_speed = 0;


	encoder_low[0] = 0;
	encoder_high[0] = 0;
	encoder_low[1] = 0;
	encoder_high[1] = 0;
	encoder_low[2] = 0;
	encoder_high[2] = 0;
	encoder_low[3] = 0;
	encoder_high[3] = 0;

last_dir = 0;
run_count = 0;
}
/* end of startup */

/**************************
** End of Initialization **
**************************/



/****************************************
**	Serial command interface functions **
****************************************/



/* deal with digital out command */
/*void digital_out_command ( void )
{
	unsigned char n = get_hex_nibble(); // get output number

	if (n > 5) return;	// range check

	unsigned shift = 1 << n; // create bit mask

	unsigned char v = get_hex_nibble(); // get value (on or off)

	if( v == 0)
	{
		shift = 255 - shift; // and mask
		PORTD &=  shift;
	}

	if (v == 1)
		PORTD |= shift;   // or mask
}*/
/* end of digital_out_command */

/* return the valur of a digital input */
/*bit get_digital_value(unsigned char n)
{
	if (n == 0) return(digital_in_0); else
	if (n == 1) return(digital_in_1); else
	if (n == 2) return(digital_in_2); else
	if (n == 3) return(digital_in_3); else
	if (n == 4) return(digital_in_4);

	return(0);
}*/
/* end of get_digital_value */


/***********************************************
**	End of serial command interface functions **
***********************************************/

/* Read data from sonar sensor */
// The parmeter is the input /output pair number
// for example passing '2' will use input 2 and output 2
//
/*void get_sonar_command( void )
{
	unsigned char n = get_hex_nibble(); 

	unsigned char mask = 1 << n;

    PORTD |= mask;   // turn output on

	delay(0x8);

	PORTD &= ~mask;   // turn output off

	TMR1H = 0;

	while(!get_digital_value(n) && TMR1H < 0x80);

	TMR1H = 0;
	TMR1L = 0;

	// Read the pulse width
	while(get_digital_value(n) && TMR1H < 0x80);

	unsigned char l = TMR1L;			// Save low order, it will change during transmittion

	send_hex(TMR1H);
	send_hex(l);
}*/
/* End of read sonar */





/* main function */
void main ( void )
{
	startup();

	setup_serial();

	while (1);	// infinte loop
}
/* end of main */