// // ECE430 - Self-navigating Lego car // Jeff Keacher & Joey Richey // November, 2003 // // Coded for PIC16F877A // //****************** // Includes //****************** #include #include "lcd.h" #include "key.h" //****************** // Constants //****************** //string literals used throughout program #define LOCATION_PROMPT "Destination:" #define GETTING_SIGNAL "Acquiring signal..." #define DESTINATION_REACHED "At Location!" //defines if we are entering the latitude or longitude #define LATITUDE 0 #define LONGITUDE 1 #define LAT_FOOT_PER_MINUTE 6 //number of feet in 1 minute of latitude #define LON_FOOT_PER_MINUTE 9 //number of feet in 1 minute of longitude at 39 degrees #define NUM_DIGITS 3 //of how many digits you can enter #define DIRECTION 3 //the location in the array which holds the direction //direction constatns #define NORTH 'N' #define SOUTH 'S' #define EAST 'E' #define WEST 'W' #define BLANK ' ' #define NUM_LOOPS_PER_TURN 10 #define NO_DATA_WAITING 0 #define SET 1 #define CLEAR 0 #define ENABLED 1 #define DISABLED 0 #define CLOCK_SPEED_MHZ 20 #define SERIAL_RATE_BPS 9600 #define SENTENCE_START '@' #define NULL_DATA '_' #define FIRST_CHAR 1 #define ZERO 0 #define POSITION_LOST 0 #define POSITION_KNOWN 1 #define LAT_MIN_START 17 #define NS_SPEED_DIR 46 #define NS_SPEED_START 49 #define EW_SPEED_DIR 41 #define EW_SPEED_START 44 #define LON_MIN_START 26 #define END_OF_SENTENCE 57 #define ASCII_NUM_OFFSET 0x30 #define MS_PER_DEGREE 175 //****************** // hardware/software interface //****************** //ports that the motors are on #define MOTOR_PORT_CTRL TRISC #define MOTOR_PORT_DIRECTION 0b11101011 #define RIGHT_MOTOR RC2 //port for thr right motor #define LEFT_MOTOR RC4 //port for the left motor #define MOTOR_PORT PORTC #define MOTORS_OFF 0b11101011 #define MOTORS_ON 0b00010100 //defintions of our keys #define BACKSPACE KEY14 #define RETURN KEY15 #define NE_KEY KEY04 #define SW_KEY KEY08 #define KEY_DIGIT_1 KEY01 #define KEY_DIGIT_2 KEY02 #define KEY_DIGIT_3 KEY03 #define KEY_DIGIT_4 KEY05 #define KEY_DIGIT_5 KEY06 #define KEY_DIGIT_6 KEY07 #define KEY_DIGIT_7 KEY09 #define KEY_DIGIT_8 KEY10 #define KEY_DIGIT_9 KEY11 #define KEY_DIGIT_0 KEY13 //register to get data from the serial port #define SERIAL_DATA_REG RCREG #define SERIAL_FLAG RCIF //****************** // Function Prototypes //****************** char digits_to_num(char *digits); void num_to_digits(unsigned int num, char* digits); void init_interrupt(void); void keypress_callback(char key); void update_LCD(void); void strcat(char *dest, char *src); void gps_add(unsigned int *ret_lat, unsigned int *ret_lon); void receive_byte(char*); void initialize_serial(void); void serial_buffer(void); void check_position(void); char ASCIItoNum(char); void turn_car(int); int arctan (int,int); int my_abs (int); void done (void); void stop_motors(void); void start_motors(void); //****************** // Global Variables //****************** //used for when user is entering input char location[4]; char cur_location; //the zero based location of the current digit you are editing char input_type; char loopindex; //change in latitude and longitude that user entered char delta_lat; char delta_lon; //the lat/lon coordinates of our destination location bank1 unsigned int dest_lat; bank1 unsigned int dest_lon; //our current lat/lon coordintates unsigned int cur_lat; unsigned int cur_lon; int _ns_speed; // Signed north/south velocity (negative number => south) int _ew_speed; // Signed east/west velocity (negative number => west) char _good_data; // A flag, zero when position stream is lost, and one when position is known char _position_updated; // A flag, which when set indicates that the position has been updated void main() { char digits[6]; //turn off the motors, just incase they are on MOTOR_PORT_CTRL = MOTOR_PORT_DIRECTION; MOTOR_PORT &=MOTORS_OFF; LCD_init(); init_interrupt(); //get location (cannot put this in a function because it overflows the stack for (loopindex=0;loopindex<4;loopindex++) { location[loopindex] = BLANK; } write_char_to_LCD(0, START_LINE_1); // start on line 1 write_string_to_LCD(LOCATION_PROMPT); input_type = LATITUDE; while (RBIE == ON) { update_LCD(); } //end get location //calculate our destination location gps_add(&dest_lat, &dest_lon); write_char_to_LCD(0, CLEAR_DISPLAY); write_string_to_LCD(GETTING_SIGNAL); // Initialize the serial port initialize_serial(); while (ON) { if (_position_updated) { //for latitude: write our current location, followed by our //destination, follow by our speed write_char_to_LCD(0, START_LINE_1); num_to_digits(cur_lat, digits); write_string_to_LCD(digits); write_char_to_LCD(1, ' '); num_to_digits(cur_lon, digits); write_string_to_LCD(digits); write_char_to_LCD(1, ' '); num_to_digits((unsigned int)_ns_speed, digits); write_string_to_LCD(digits); //for longitude on line 2: write our current location, followed by our //destination, follow by our speed write_char_to_LCD(0, START_LINE_2); num_to_digits(dest_lat, digits); write_string_to_LCD(digits); write_char_to_LCD(1, ' '); num_to_digits(dest_lon, digits); write_string_to_LCD(digits); write_char_to_LCD(1, ' '); num_to_digits((unsigned int)_ew_speed, digits); write_string_to_LCD(digits); } check_position(); } } // // Routine to handle all interrupts and call the appropriate one // #pragma interrupt_level 1 void interrupt inter () { // interupt handler if (RBIF && PEIE != ON) { handle_keyevent(keypress_callback); } // If the serial port has data, call the serial buffer function if (SERIAL_FLAG && PEIE) { serial_buffer(); SERIAL_FLAG = NO_DATA_WAITING; } } // // Initilize the interrupts that we need for the program // void init_interrupt(void) { // key strokes PORTB = 0; //ckear portb RBPU = OFF; //enable PORTB pullup resistors TRISB = ENABLE_KEYPAD_INTERRUPT; // Set RB3-0 as output PORTB = ENABLE_KEYPAD_INTERRUPT; RBIE = ON; // turn on RB PORT Change interrupt RBIF = OFF; // Clear interrupt flag GIE = ON; // enable global interupt } // // This function is called after a key has been pressed (and succesfully debounced) // The parameter is the key that is pressed as defined in key.h--KEY01-KEY16 void keypress_callback(char key) { char value; if (key == NO_KEY_PRESSED) { return; } else if (key == BACKSPACE) { if (cur_location == 0) { location[cur_location] = BLANK; } else { location[cur_location - 1] = BLANK; cur_location--; } } else if (key == RETURN) { if (location[DIRECTION] != BLANK) { if (input_type == LATITUDE) { //save the state of longitude delta_lat = digits_to_num(location); for (loopindex=0;loopindex<4;loopindex++) { location[loopindex] = BLANK; } cur_location = 0; input_type = LONGITUDE; } else { //longitude //save the state of the latitude delta_lon = digits_to_num(location); //done getting input, don't listen for key presses anymore RBIE = OFF; } } } else if (key == NE_KEY) { if (input_type == LONGITUDE) { location[DIRECTION] = EAST; } else { location[DIRECTION] = NORTH; } } else if (key == SW_KEY) { if (input_type == LONGITUDE) { location[DIRECTION] = WEST; } else { location[DIRECTION] = SOUTH; } } else { // a number if ((cur_location < NUM_DIGITS -1) || (location[2] == BLANK)) { switch (key) { case KEY_DIGIT_1: value = '1'; break; case KEY_DIGIT_2: value = '2'; break; case KEY_DIGIT_3: value = '3'; break; case KEY_DIGIT_4: value = '4'; break; case KEY_DIGIT_5: value = '5'; break; case KEY_DIGIT_6: value = '6'; break; case KEY_DIGIT_7: value = '7'; break; case KEY_DIGIT_8: value = '8'; break; case KEY_DIGIT_9: value = '9'; break; case KEY_DIGIT_0: value = '0'; break; default: value = BLANK; } location[cur_location] = value; cur_location++; } } } // // Updates the LCD while input is being entered // void update_LCD() { write_char_to_LCD(0, START_LINE_2); // start on line 2 write_char_to_LCD(1, location[0]); write_char_to_LCD(1, location[1]); write_char_to_LCD(1, location[2]); write_char_to_LCD(1, ' '); write_char_to_LCD(1, location[DIRECTION]); } // //converts an interger to an array of characters // void num_to_digits(unsigned int num, char* digits) { unsigned int factor = 10000; char i = 0; digits[5] = '\0'; while (factor > 0) { digits[i] = num/factor + '0'; num %= factor; i++; factor /=10; } } // //converts a character array to 3 numbers to its corresponding integer value // char digits_to_num(char *digits) { int i; char factor = 1; char retval=0; for (i=2;i>=0;i--) { if (digits[i] != BLANK) { retval+= (digits[i]-'0') * factor; factor *=10; } } return retval; } // // Adds the offset provided by the user to the current location, so that we know where we need to stop at // void gps_add(unsigned int *ret_lat, unsigned int *ret_lon) { while(cur_lat == 0); *ret_lat = cur_lat + (delta_lat/LAT_FOOT_PER_MINUTE); *ret_lon = cur_lon + (delta_lon/LON_FOOT_PER_MINUTE); } // check_position: Compare the current position to the desired position. Adjust course accordingly. // Uses _position_updated flag to determine if the position has been updated since the last time it // was called. This prevents the function from continually correcting course based on a previous location. void check_position(void) { int ns_diff; // difference between source and destination int ew_diff; // difference between source and destination int bearing_dir; // the direction for the bearing int cur_dir; // the current direction static char turn_count = 0; if (_good_data == CLEAR) { // We don't have a solid location. Stop the motors. stop_motors(); } else { // We have a solid location. Start the motors. start_motors(); } // Only check the position if it has changed since last time if (_position_updated == SET) { // Find the angle between the direction we're going and the direction we need to go // First, find the bearing from the current location to the destination ns_diff = dest_lat - cur_lat; // amount north we need to go ew_diff = dest_lon - cur_lon; // amount east we need to go // Check if we are at our destination if (ns_diff == ZERO && ew_diff == ZERO) { // If we made it, we're done. done(); } if (ew_diff != ZERO) // we don't want to divide by zero { bearing_dir = arctan(ns_diff,ew_diff); } // Second, find the current direction (this parallels the above as long as we're relatively // close to the equator [things basically square]) if (_ew_speed != ZERO) { cur_dir = arctan(_ns_speed,_ew_speed); } if (++turn_count % NUM_LOOPS_PER_TURN == 0) { turn_count = 0; // Finally, turn the car to the desired angle by turning it an amount equal to // the difference between the two angles turn_car(cur_dir - bearing_dir); } } // Clear the _position_updated flag so that we don't run this again unnecessarily _position_updated = CLEAR; } // done: the car has found its destination void done (void) { // Found the destination // Display "Done" on the LCD write_char_to_LCD(0, CLEAR_DISPLAY); write_string_to_LCD(DESTINATION_REACHED); // Stop. stop_motors(); // Loop forever while (ON) {}; } // stop_motors: All stop. void stop_motors(void) { // stop the motors (both of them) MOTOR_PORT &=MOTORS_OFF; } // start_motors: All start. void start_motors(void) { // start the motors (both of them) MOTOR_PORT |=MOTORS_ON; } // turn_car: turn the car the specified number of degrees void turn_car(int angle) { unsigned int i; // Relate the angle to a time // Use the MS_PER_DEGREE constant to determine the number of miliseconds to be turning LEFT_MOTOR = OFF; for(i=0;i my_abs(numerator) ) { ratio = my_abs(denominator / numerator); need90shift = SET; } else { ratio = my_abs(numerator / denominator); } // This algorithm is based off a PASCAL routine from http://www.convict.lu/Jeunes/Math/arctan.htm work = ((-150 + 310*ratio - (ratio*ratio/2) - (ratio*ratio/3))/50 + 5)/10; if (need90shift == SET) // If the angle is above 45 degrees, it needs special treatment { work = 90 - work; } if (numerator >= 0 && denominator > 0) { // first quadrant work = 90 - work; } else if (numerator >= 0 && denominator < 0 ) { // In second quadrant work = 270 + work; } else if (numerator < 0 && denominator < 0 ) { // third quadrant work = 270 - work; } else if (numerator < 0 && denominator > 0 ) { // fourth quadrant work = 90 + work; } } else { // Case if the denominator is zero (the angle is 0 or 180 degrees (compass direction) if (numerator >= 0) // Or equal, so that it goes straight { // Straight ahead work = 0; } else { // Otherwise, must be straight behind work = 180; } } return work; } // my_abs: cheap absolute value function int my_abs(int input) { if (input >= 0) { return input; } else { return -input; } } // serial_buffer: Read in a sentence of the Garmin "simple text" position data void serial_buffer(void) { // Local static variables // sentence_position: Current character being worked on in the Simple Text sentence. One-index. // A value of zero means that the position is not yet known (as when the device first starts up) // The value is also set to zero when the null position character is received from the GPS ('_') // indicating that the GPS has lost its position. static unsigned char sentence_position = ZERO; // These position variables are kept private until a complete new location is known, at // which point, they are copied into the global position variables static unsigned int flip_lat_minutes; static unsigned int flip_lon_minutes; static char flip_ns_speed; // Signed north/south velocity (negative number => south) static char flip_ns_speed_dir; static char flip_ew_speed; // Signed east/west velocity (negative number => west) static char flip_ew_speed_dir; // Normal local variables char cwork; // Character pulled from serial port that has yet to be processed // Get the byte of data from the serial port receive_byte(&cwork); // write_char_to_LCD(1, cwork); // Check if there is a new sentence if (cwork == SENTENCE_START) { _good_data = POSITION_KNOWN; sentence_position = FIRST_CHAR; } // Check if the position is lost if (cwork == NULL_DATA) { // First, set the flag so that the motors stop _good_data = POSITION_LOST; // Next, set the sentence position to zero sentence_position = ZERO; } // Convert ASCII to a number cwork = ASCIItoNum(cwork); // Switch based on sentence position switch (sentence_position) { case LAT_MIN_START: flip_lat_minutes = 10000*cwork; break; case LAT_MIN_START+1: flip_lat_minutes += 1000*cwork; break; case LAT_MIN_START+2: flip_lat_minutes += 100*cwork; break; case LAT_MIN_START+3: flip_lat_minutes += 10*cwork; break; case LAT_MIN_START+4: flip_lat_minutes += cwork; break; case LON_MIN_START: flip_lon_minutes = 10000*cwork; break; case LON_MIN_START+1: flip_lon_minutes += 1000*cwork; break; case LON_MIN_START+2: flip_lon_minutes += 100*cwork; break; case LON_MIN_START+3: flip_lon_minutes += 10*cwork; break; case LON_MIN_START+4: flip_lon_minutes += cwork; break; case EW_SPEED_DIR: flip_ew_speed_dir = cwork; break; case EW_SPEED_START: flip_ew_speed = 10 * cwork; break; case EW_SPEED_START+1: flip_ew_speed += cwork; break; case NS_SPEED_DIR: flip_ns_speed_dir = cwork; break; case NS_SPEED_START: flip_ns_speed = 10 * cwork; break; case NS_SPEED_START+1: flip_ns_speed += cwork; break; case END_OF_SENTENCE: // If we are at the end of the sentence, copy the temporary position values into the // the global position variables cur_lat = flip_lat_minutes; cur_lon = flip_lon_minutes; if (flip_ew_speed_dir == SET) { // East _ew_speed = flip_ew_speed; } else { // West _ew_speed = -flip_ew_speed; } if (flip_ns_speed_dir == SET) { // North _ns_speed = flip_ns_speed; } else { // South _ns_speed = -flip_ns_speed; } _position_updated = SET; default: break; } // Increment the sentence position sentence_position++; } // ASCIItoNum: Converts the ASCII representation of a number to a binary representation // Note: no boundary checking char ASCIItoNum(char input) { char localWork; localWork = input - ASCII_NUM_OFFSET; return localWork; } // initialize_serial: Sets up the serial port to be able to receive // void initialize_serial(void) { // Set the flags SPEN = ENABLED; // serial port SYNC = DISABLED; BRGH = ENABLED; // high speed RX9 = DISABLED; CREN = ENABLED; // continuous mode RCIE = ENABLED; // enable interrupts // NOTE: No data direction need be specified, as PORTC // defaults to being an input // Set up the baud rate SPBRG = (1000000/SERIAL_RATE_BPS*CLOCK_SPEED_MHZ - 16)/16; PEIE = ON; // peripheral interrupts return; } // receive_byte: Returns a byte from the serial port in the // function parameter pointer // void receive_byte(char* pSerialByte) { // Wait for a byte to be received while (SERIAL_FLAG == NO_DATA_WAITING) {}; // When there's a byte ready, copy it *pSerialByte = SERIAL_DATA_REG; PORTD = SERIAL_DATA_REG; // Clear the byte-received flag SERIAL_FLAG = NO_DATA_WAITING; return; } //receive_byte()