#include #include #pragma CLOCK_FREQ 20000000 //#pragma DATA 0x2007, _HS_OSC & _CP_OFF & _WRT_OFF & _CPD_OFF & _LVP_ON & _BODEN_ON & _PWRTE_ON & _WDT_OFF #pragma DATA 0x2007, _CP_OFF & _WDT_OFF & _BODEN_OFF & _PWRTE_ON & _HS_OSC & _LVP_OFF & _CPD_OFF //Define the analog inputs from the sensors #define left_eye _porta,0 #define rght_eye _porta,1 //#define _porta,2 //#define _porta,3 //#define _porta,4 //#define _porta,5 //Define control outputs for right motors #define motdir0a _portb,0 #define motdir0b _portb,1 #define motdir1a _portb,2 #define motdir1b _portb,3 //#define _portb,4 //#define _portb,5 //#define _portb,6 //#define _portb,7 //Define the SPI port #define fl_line _portc,0 #define fr_line _portc,1 #define DAT _portc,2 #define CLOCK _portc,3 #define LATCH _portc,4 //#define _portc,5 //#define _portc,6 //#define _portc,7 #define FR_GREEN 0x0001 //1 #define FR_BLUE 0x0002 //2 #define FR_RED 0x0004 //3 #define BLUE0 0x0008 //4 #define BLUE1 0x0010 //5 #define BLUE2 0x0020 //6 #define BLUE3 0x0040 //7 #define FL_RED 0x0080 //8 #define FL_GREEN 0x0100 //9 #define FL_BLUE 0x0200 //10 #define BL_RED 0x0400 //11 #define BL_GREEN 0x0800 //12 #define BL_BLUE 0x1000 //13 #define BR_GREEN 0x2000 //14 #define BR_BLUE 0x4000 //15 #define BR_RED 0x8000 //16 //Define the bits in the command_byte #define mot0dir 7 #define mot1dir 6 #define EYE_THRESHOLD 80 #define LEFT_TURN 0b10000000 #define RIGHT_TURN 0b01000000 #define BACKWARD 0b00000000 #define FORWARD 0b11000000 #define LINE_TURN 0b11100000 //Define some values for sampling different A2D channels #define chan0 0b10000001 //select channel AN0 on PA0 #define chan1 0b10001001 //select channel AN1 on PA1 #define chan2 0b10010001 //select channel AN2 on PA2 #define chan3 0b10011001 //select channel AN3 on PA3 #define chan4 0b10100001 //select channel AN4 on PA5 typedef enum{ ATTACK, SURVIVE, SEARCH, RUN, LINE, RIGHT, LEFT, OFF } states; //These are the global vars that would normaly go in the cblock char counter = 0; char temp = 0; char command_byte = 0; //holds motor speeds and directions char next_command = 0; char motorcount = 0; char mot0speed = 0; char mot1speed = 0; //cycle that is being pushed out on unsigned char currentState = SEARCH; unsigned char lastState = OFF; unsigned char botLastSeenOn = OFF; unsigned char left_eye_val = 0; unsigned char right_eye_val = 0; unsigned char left_line_val = 0; unsigned char right_line_val = 0; unsigned short average = 0; #define MAX_SAMPLES 8 #define MAX_LINE_SAMPLES 4 unsigned char sample = 0; unsigned char leftArray[MAX_SAMPLES]; unsigned char rightArray[MAX_SAMPLES]; unsigned char leftLineArray[MAX_SAMPLES]; unsigned char rightLineArray[MAX_SAMPLES]; void interrupt ( void ) { clear_bit( intcon, GIE ); //Disable interrupts motorcount++; if( motorcount >= 10 ) { motorcount = 0; } //SetPWM on motor0 if( test_bit( command_byte, mot0dir ) ) { //Forward on motor0 clear_bit( portb,0 ); clear_bit( portb,1 ); if( mot0speed > motorcount ) { set_bit( portb,0 ); } } else { //Backward on motor0 clear_bit( portb,0 ); clear_bit( portb,1 ); if( mot0speed > motorcount ) { set_bit( portb,1 ); } } //SetPWM on motor1 if( test_bit( command_byte, mot1dir ) ) { //Forward on motor1 clear_bit( portb,2 ); clear_bit( portb,3 ); if( mot1speed > motorcount ) { set_bit( portb,2 ); } } else { //Backward on motor1 clear_bit( portb,2 ); clear_bit( portb,3 ); if( mot1speed > motorcount ) { set_bit( portb,3 ); } } clear_bit( intcon, 2 ); //Clear the interup flag set_bit( intcon, GIE ); //Enable interrupts } //***************************** ser_out ****************************** // This routine outputs single bytes on the SPI bus // w holds value to be pushed out void spiOutput ( char n ) { temp = n; _asm { movlw d'8' //Load counter for one byte output movwf _counter movlw d'0' nextbit: movf _temp, W //Load value to shift out btfsc _temp, 0 //Is current output a one? goto output_1 //Yes output 1 goto output_0 //No output 0 output_0: bcf DAT nop nop bsf CLOCK nop bcf CLOCK goto rest output_1: bsf DAT //clock output line nop nop bsf CLOCK nop bcf CLOCK bcf DAT rest: rrf _temp, W //Rotate Right movwf _temp decfsz _counter,F //Are we done yet goto nextbit return } } //;---------------------------------------------------------- //***************************** latch_it ***************************** // Used to latch the output line ofthe serial led driver void latchIt ( void ) { _asm { bcf LATCH nop bsf LATCH nop bcf LATCH return } } void setLeds ( short leds ) { spiOutput ( (leds & 0x00FF) ); spiOutput ( (char)((leds >> 8) & 0x00FF) ); latchIt(); } //---------------------------------------------------------- // Transmit a byte out the UART // Returns on sucsessful completion // outGoing holds byte to be sent void sendByte ( char outGoing ) { _asm { movf _outGoing, W movwf _txreg bsf _status,RP0 //RAM PAGE 1 next: btfss _txsta,TRMT //done sending goto next //no, wait bcf _status,RP0 //RAM PAGE 0 return } } void rightAverage ( void ) { average= 0; for (temp = 0; temp < MAX_SAMPLES; temp++) { average += rightArray[temp]; } right_eye_val = average >> 3; } void leftAverage ( void ) { average= 0; for (temp = 0; temp < MAX_SAMPLES; temp++) { average += leftArray[temp]; } left_eye_val = average >> 3; } void rightLineAverage ( void ) { average= 0; for (temp = 0; temp < MAX_SAMPLES; temp++) { average += rightLineArray[temp]; } if( average >= 6 ) right_line_val = 1; else right_line_val = 0; } void leftLineAverage ( void) { average= 0; for (temp = 0; temp < MAX_SAMPLES; temp++) { average += leftLineArray[temp]; } if( average >= 6 ) left_line_val = 1; else left_line_val = 0; } void getEyeReadings ( void ) { sample++; if( sample >= MAX_SAMPLES ) { sample = 0; } adcon0 = chan0; delay_us (100); set_bit( adcon0, GO ); while ( test_bit( adcon0,GO )); //Wait for conversion to complete leftArray[sample] = adresh; leftAverage(); adcon0 = chan1; delay_us (100); set_bit( adcon0, GO ); while ( test_bit( adcon0,GO )); //Wait for conversion to complete rightArray[sample] = adresh; rightAverage(); // rightLineArray[sample] = !test_bit( portc, 0 ); // rightLineAverage(); right_line_val = !test_bit( portc, 0 ); // leftLineArray[sample] = !test_bit( portc, 1 ); // leftLineAverage(); left_line_val = !test_bit( portc, 1 ); } //---------------------------------------------------------- // Transmit a byte out the UART // Returns on sucsessful completion // outGoing holds byte to be sent void report ( void ) { sendByte ('R'); sendByte (':'); sendByte ( right_eye_val ); sendByte ( 0x09 ); sendByte ('L'); sendByte (':'); sendByte ( left_eye_val ); sendByte ( 0x09 ); sendByte ( 0x0D ); } void init ( void ) { clear_bit(status, RP0); //Goto bank 0 porta = 0b00000000; //PORTA 0x00 portb = 0b00000000; //PORTB 0x00 portc = 0b01000000; //PORTC 0x40 adcon0 = 0b01000001; //Setup A2D on all portA pins and set osc. bits rcsta = 0b10010000; //Setup the RXTSA register for RS232 set_bit(status, RP0); //Goto bank 1 trisa = 0b00011111; //PORTA is all inputs; trisb = 0b11110000; //PORTB(0,1,2,3) outputs (4,5,6,7) inputs; trisc = 0b10100011; //PORTC(0,1,5,7) inputs (2,3,4,6) output; adcon1 = 0b00001001; //Setup A2D right justified txsta = 0b10100100; //Setup the TXSTA register for RS232 spbrg = 10; //Setup the baud rate generator for 115k at 20MHz option_reg = 0b10000001;//Set timer prescaler to 1/4 clear_bit(status, RP0); //Goto bank 0 set_bit(intcon, 5); //Enable the timer interupt clear_bit(intcon, 2); //Clear the interup flag set_bit(intcon, GIE); //Enable interrupts _asm { movf _rcreg,W movf _rcreg,W movf _rcreg,W //flush receive buffer //allow time for the uart to settle clrf _temp Settle: decfsz _temp, F goto Settle } } void main() { init(); char i = 0; short ledVal = 0; //clear the motor control bits command_byte = FORWARD; motorcount = 0x00; mot0speed = 0x00; mot1speed = 0x00; ledVal = BLUE0; setLeds( ledVal ); for( i = 0; i < 5; i++ ) { delay_ms( 250 ); delay_ms( 250 ); delay_ms( 250 ); delay_ms( 250 ); ledVal <<= 1; setLeds( ledVal ); } ledVal = ( FL_GREEN | FR_GREEN ); setLeds( ledVal ); command_byte = FORWARD; mot0speed = 0x0A; mot1speed = 0x0A; delay_ms( 250 ); delay_ms( 250 ); command_byte = BACKWARD; mot0speed = 0x0A; mot1speed = 0x0A; delay_ms( 250 ); delay_ms( 250 ); mot0speed = 0; mot1speed = 0; while(1) { switch( currentState ) { case ATTACK: command_byte = FORWARD; mot0speed = 0x0A; mot1speed = 0x0A; while (currentState == ATTACK ) { if( left_eye_val > EYE_THRESHOLD && right_eye_val > EYE_THRESHOLD ) // { ledVal = ( BLUE1 | BLUE2 | FR_RED | FL_RED | BR_RED | BL_RED ); command_byte = FORWARD; botLastSeenOn = RIGHT; mot0speed = 0x0A; mot1speed = 0x0A; } else { if( right_line_val && left_line_val ) //Both front high { ledVal = ( FR_RED | FL_RED | BR_GREEN | BL_GREEN); next_command = LINE_TURN; currentState = LINE; lastState = ATTACK; } else if( right_line_val ) //Just Right high { ledVal = ( FL_GREEN | FR_RED | BR_GREEN | BL_GREEN ); next_command = LEFT_TURN; currentState = LINE; botLastSeenOn = LEFT; lastState = ATTACK; } else if( left_line_val ) //Just Left high { ledVal = ( FL_RED | FR_GREEN | BR_GREEN | BL_GREEN ); next_command = RIGHT_TURN; currentState = LINE; botLastSeenOn = RIGHT; lastState = ATTACK; } else //No line eyes for other bot { if( left_eye_val > EYE_THRESHOLD && right_eye_val > EYE_THRESHOLD ) // { ledVal = ( BLUE1 | BLUE2 | FR_RED | FL_RED | BR_RED | BL_RED ); command_byte = FORWARD; botLastSeenOn = RIGHT; mot0speed = 0x0A; mot1speed = 0x0A; } else if( left_eye_val > EYE_THRESHOLD && right_eye_val < EYE_THRESHOLD ) // { ledVal = ( BLUE0 | FR_RED | FL_RED | BR_RED | BL_RED ); command_byte = FORWARD; botLastSeenOn = RIGHT; mot0speed = 0x07; mot1speed = 0x0A; } else if ( left_eye_val < EYE_THRESHOLD && right_eye_val > EYE_THRESHOLD ) // { ledVal = ( BLUE3 | FR_RED | FL_RED | BR_RED | BL_RED ); botLastSeenOn = LEFT; command_byte = FORWARD; mot0speed = 0x0A; mot1speed = 0x07; } else if ( left_eye_val < EYE_THRESHOLD && right_eye_val < EYE_THRESHOLD ) //Lost target search again { ledVal = ( FR_RED | FL_RED | BR_RED | BL_RED ); currentState = SEARCH; lastState = ATTACK; } else //Still in front full speed ahead { ledVal = ( BLUE1 | BLUE2 | FR_RED | FL_RED | BR_RED | BL_RED ); command_byte = FORWARD; mot0speed = 0x0A; mot1speed = 0x0A; } } } delay_ms( 10 ); setLeds( ledVal ); getEyeReadings ( ); //report(); } break; case SEARCH: while ( currentState == SEARCH ) { ledVal = ( FL_GREEN | FR_GREEN | BR_GREEN | BL_GREEN ); if( botLastSeenOn == LEFT ) { mot0speed = 0x04; mot1speed = 0x08; command_byte = FORWARD; } else if( botLastSeenOn == RIGHT ) { mot0speed = 0x08; mot1speed = 0x04; command_byte = FORWARD; } else { mot0speed = 0x08; mot1speed = 0x04; command_byte = FORWARD; } if( right_line_val && left_line_val ) //Both front high { ledVal = ( FL_RED | FR_RED | BR_GREEN | BL_GREEN ); next_command = LINE_TURN; currentState = LINE; lastState = SEARCH; } else if( left_line_val ) //Just left high { ledVal = ( FL_RED | FR_GREEN | BR_GREEN | BL_GREEN ); next_command = RIGHT_TURN; currentState = LINE; lastState = SEARCH; } else if( right_line_val ) //Just right high { ledVal = ( FL_GREEN | FR_RED | BR_GREEN | BL_GREEN ); next_command = LEFT_TURN; currentState = LINE; lastState = SEARCH; } else //No line check eyes for other bot { if( left_eye_val > EYE_THRESHOLD ) { ledVal = ( BLUE2 | FR_RED | FL_RED | BR_RED | BL_RED ); currentState = ATTACK; } else if ( right_eye_val > EYE_THRESHOLD ) { ledVal = ( BLUE1 | FR_RED | FL_RED | BR_RED | BL_RED ); currentState = ATTACK; } } delay_ms( 10 ); getEyeReadings ( ); setLeds( ledVal ); //report(); } break; case LINE: if( next_command == LINE_TURN ) { command_byte = BACKWARD; mot0speed = 0x0A; mot1speed = 0x0A; delay_ms( 250 ); delay_ms( 250 ); command_byte = LEFT_TURN; mot0speed = 0x0A; mot1speed = 0x0A; delay_ms( 250 ); delay_ms( 250 ); } else { command_byte = next_command; mot0speed = 0x0A; mot1speed = 0x0A; delay_ms( 250 ); delay_ms( 250 ); delay_ms( 250 ); } currentState = lastState; ledVal = ( FR_GREEN | FL_GREEN | BR_GREEN | BL_GREEN); setLeds( ledVal ); break; default: break; } getEyeReadings ( ); //report(); } }