map_bot.c

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

/*
    Robot Controller Source
	(C) 2006 Jason Hunt [e-mail]

	Version 0.01

	16f871.h has been modified as follows:
	
	// original
	// #pragma chip PIC16F871, core 14, code 2048, ram 32 : 0xBF /0 /3

	#pragma chip PIC16F871, core 14, code 0x1F00, ram 32 : 0x1FF
	#pragma cdata[0] = 0x0180 + 0x0A // CLRF PCLATH, prepare for code page change
	#pragma cdata[1] = 0 // NOP
	#pragma cdata[2] = 0 // NOP
	#pragma resetVector 3 // start address for user program

	This reserves space for the serial bootloader.
*/

#include <int16cxx.h>

#pragma chip PIC16f871
#pragma config |= 0x3FF2 			// For 20 mhz ceramic resinator


#define NUM_RECV_BYTES	11			// Bytes in control packet
#define NUM_SEND_BYTES	20			// Bytes in telemetery packet

#define NUM_SERVOS	6	   		    // Number of servo outputs

#define NUM_ANALOG 8

/* Pin Assignments */

#pragma bit led_0 @ PORTB.6
#pragma bit led_1 @ PORTB.7

//#pragma bit encoder_0 @ PORTB.0 // digital in 0
//#pragma bit encoder_1 @ PORTA.4
//#pragma bit encoder_2 @ PORTE.0
//#pragma bit encoder_3 @ PORTE.1

#pragma bit encoder_0 @ PORTD.0 // digital in 0
#pragma bit encoder_1 @ PORTD.1
#pragma bit encoder_2 @ PORTD.2
#pragma bit encoder_3 @ PORTD.3


//#pragma bit digital_in_4 @ PORTE.2

//#pragma bit radio_in_0 @ PORTD.6
//#pragma bit radio_in_1 @ PORTD.7

//#pragma bit open_collector_0 @ PORTB.4
//#pragma bit open_collector_1 @ PORTB.5

/* Pin assignments for PWM */
#pragma bit pwm_out_0 @ PORTB.1
#pragma bit pwm_out_1 @ PORTB.2
#pragma bit pwm_out_2 @ PORTB.5
#pragma bit pwm_out_3 @ PORTB.4

/* Data buffers for serial */
unsigned char recv_buffer[NUM_RECV_BYTES];
unsigned char send_buffer[NUM_SEND_BYTES];

unsigned char servo_pos[NUM_SERVOS];		// Servo Positions
unsigned char pwm_value[4];					// PWM Dutysw

unsigned char analog_low[8];				// Analog low bytes
unsigned char analog_high[8];				// Analog high bytes


unsigned char cur_analog;					// Current analog capture

unsigned char servo_mode;			// Current state of the servo interrupt system
unsigned char cur_servo;			// Current servo 
unsigned char pwm_cycle;			// CUrrent PWM cutoff value

unsigned char cur_recv_byte;		// Index of recv buffer
unsigned char cur_send_byte;		// Index of send buffer
unsigned char read_timeout;			// Timeout counter
bit reading_data;					// True if reading

unsigned char wait_analog;			// True if waiting to start converstion

unsigned char update_timeout;

unsigned char running_counter;
unsigned char recv_counter;
bit last_valid;


bit last_encoder_0;
bit last_encoder_1;
bit last_encoder_2;
bit last_encoder_3;



unsigned char last_dir;

unsigned char encoder_0_count;
unsigned char encoder_1_count;
unsigned char encoder_2_count;
unsigned char encoder_3_count;

unsigned char encoder_0_speed;


unsigned char encoder_low[4];
unsigned char encoder_high[4];


unsigned char run_count;

/*******************
** Interrupt Code **
*******************/

#pragma interruptSaveCheck  n
#pragma origin 4





/* the ISR to update the servos */
interrupt ISR(void)
{
    int_save_registers

    if (RCIF)
    {
        RCIF = 0;
		reading_data = 1;
		read_timeout = 0;

		// Eat data and timeout if sync is off
		if (!last_valid)
		{
			recv_buffer[cur_recv_byte++] = RCREG;  
			cur_recv_byte = 0;
		}


		// Send current telemetry data for the first char recevied
        // This will utilize full duplex for better transfer
		if (cur_recv_byte == 0 && last_valid)
		{	   
            // Package current telemetry data for transmittion
			// Start byte
		    send_buffer[0] = 0xff;

			// Low bytes of analog data
			send_buffer[1] = analog_low[0];
			send_buffer[2] = analog_low[1];
			send_buffer[3] = analog_low[2];
			send_buffer[4] = analog_low[3];
			send_buffer[5] = analog_low[4];
			send_buffer[6] = analog_low[5];
			send_buffer[7] = analog_low[6];
			send_buffer[8] = analog_low[7];

			// Shift MSB of analog data to fit into two bytes
			send_buffer[9] = analog_high[0] | (analog_high[1]<<2) ;
			send_buffer[9] += analog_high[2]<<4;

			unsigned int tmp = analog_high[3] * 64;
			send_buffer[9] += tmp;


			send_buffer[10] = analog_high[4] | (analog_high[5]<<2) ;
			send_buffer[10] += analog_high[6]<<4;

			tmp = analog_high[7] * 64;
			send_buffer[10] += tmp;

//			send_buffer[7] = analog_high[3] | (analog_high[4]<<2);

			// Other telemetry data
//			send_buffer[8] =run_count;// encoder_0_speed;

			send_buffer[11] = encoder_low[0];
			send_buffer[12] = encoder_high[0];

			send_buffer[13] = encoder_low[1];
			send_buffer[14] = encoder_high[1];

			send_buffer[15] = encoder_low[2];
			send_buffer[16] = encoder_high[2];

			send_buffer[17] = encoder_low[3];
			send_buffer[18] = encoder_high[3];

			// Stop byte
			send_buffer[19] = 0xff;

			TXIE = 1;					// Enable transmit to send packet 
		} 

		// Last char in packet
        if (cur_recv_byte >= NUM_RECV_BYTES)
        {
			cur_recv_byte = 0;


            if (recv_buffer[0] == 0xff && recv_buffer[NUM_RECV_BYTES-1] == 0xff)
            {
				// first byte is For digital controls.

				unsigned char dir = recv_buffer[1] & 0x03;

				if (dir != last_dir && recv_buffer[2] != 0 && recv_buffer[3] != 0)
				{
	led_0 = !led_0;
					last_dir = dir;

					encoder_high[0] = 0;
					encoder_low[0] = 0;

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

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

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

				}

				if (recv_buffer[1] & 0x01)
				{
					pwm_value[0] = recv_buffer[2];
					pwm_value[1] = 0;										
				}
				else
				{
					pwm_value[0] = 0;
					pwm_value[1] = recv_buffer[2];
				}

				if (recv_buffer[1] & 0x02)
				{
					pwm_value[2] = recv_buffer[3];
					pwm_value[3] = 0;
				}
				else
				{
					pwm_value[2] = 0;
					pwm_value[3] = recv_buffer[3];
				}


				// Copy servo data
				servo_pos[0] = recv_buffer[4];
				servo_pos[1] = recv_buffer[5];
				servo_pos[2] = recv_buffer[6];
				servo_pos[3] = recv_buffer[7];
				servo_pos[4] = recv_buffer[8];
				servo_pos[5] = recv_buffer[9];


				recv_counter++;
	
				if (recv_counter >= 10)
				{
					recv_counter = 0;
					//led_1 = !led_1;
				}


				last_valid = 1;
             } 
			else 
				last_valid = 0;

			reading_data = 0;

			update_timeout = 0; // Reset update timeout


		}
        else
    		recv_buffer[cur_recv_byte++] = RCREG;  // Normal char for packet
    } 

    // Ready for TX byte
    if (TXIF && TXIE)
    {
		// End of packet?
		if(cur_send_byte >= NUM_SEND_BYTES)
        {
			cur_send_byte = 0;					// Reset to first byte for next send
            TXIE = 0;							// Last byte has been sent, disable tx
		}
		else
			TXREG = send_buffer[cur_send_byte++]; // Send next byte
    }


    // check for timer zero interrupt
	if (T0IF)
	{

		if (servo_mode == 0) // setupup for base pos wait
		{
			servo_mode = 1; 					   // next trigger we will setup for base delay
			OPTION = 4;   					   // reset back into 1:32 prescaled mode

			TMR0 = 0xa0;   // set base time delay		
			
			unsigned char s = 1 << cur_servo; // create mask

	   		PORTC |= s; // turn this one on

		} else
		if (servo_mode == 1) 					// setup for position hold
		{
    		servo_mode = 2;					// next mode will turn off and check for long delay
			TMR0 = servo_pos[cur_servo];	// reset the timer to the pos
	
		} else
		if (servo_mode == 2)					// setupfor next servo
		{
			servo_mode = 0;						// reset back to base mode

			unsigned char s = 1 << cur_servo;  // create mask

	  	    PORTC &= ~s; // turn the current servo off
	
			cur_servo++; // next servo
		
			// Setup longer delay to fill the 20 ms window, 
			// we need to burn 11ms on average
			// 5Mhz clock, 0.0512 MS per increment at 1:256 prescaler
			// 11ms / 0.0512 ms = 215 cycles to burn, start time at 40

			if (cur_servo == NUM_SERVOS)
			{
				cur_servo = 0;	// reset back the first servo

				OPTION = 7;

				TMR0 = 40;		// set the value for the longer delay
			}
		}

	    T0IF = 0; // clear timer zero interrupt
	} 

    // check for timer one interrupt
	if (TMR1IF)	
	{
		TMR1L = 0x00;			// reset timer
		TMR1H = 0x80;

		running_counter++;





		if (running_counter >= 20)
		{
run_count++;
			encoder_0_speed = encoder_0_count;
			encoder_0_count = 0;
	led_1 = !led_1;
			running_counter = 0;
		//	led_0 = !led_0;
		}

        if (reading_data)
        {
			read_timeout++;

			// The read has timed out
			if (read_timeout >= 4)
			{
				reading_data = 0;				// reset reading
				cur_recv_byte = 0;				// reset to current byte
				last_valid = 1;
	       }
		}


		// Update time out
		update_timeout++;

		// Too many timeouts have passed, kill motors
		if (update_timeout >= 30)