/**************************************************************************** Title : Koolio main code - REV5 Author: Gorang Gandhi (gorang@ufl.edu) Date: 5/4/2007 Software: AVR-GCC Target: Atmega 128 Comments: Code for Koolio to obstacle avoid and go from a start point through the door and to Schwartz/Arroyo office and back. Better obstacle avoidance than REV3. Added Top Center Sonar. Takes in a command from SBC to go to which office or remote mode. *****************************************************************************/ #include #include #include #include #include #include #include #include #include #include "i2c.h" #include "srf08.h" #include "lcd.h" #include "compass.h" #include "interrupt.h" #define OCR_1MS 250 /**************I2C ADDRESS DECLARATIONS********************/ // Motor Drivers #define LEFT_MOTOR 0xB0 #define RIGHT_MOTOR 0xB2 // Sonar #define SNR_BL 0xE0 #define SNR_CL 0xE2 #define SNR_CR 0xE4 #define SNR_BR 0xE6 #define SNR_BK 0xE8 #define SNR_TL 0xEA #define SNR_TR 0xEC #define SNR_TC 0xEE // Compass #define CMP 0x60 //Motor Driver Constants #define FORWARD 1 //Speeds commands #define STOP 0 //for the #define REVERSE 2 //command register //Motor Driver Registers #define COMMAND 0 //R/W #define STATUS 1 //Read only #define SPEED 2 //R/W 0-255 #define ACCEL 3 //R/W 0-255 #define TEMP 4 #define CURRENT 5 //Serial Settings #define BAUD 9600L /* use 9600 baud for UART0 connection */ #define CPU_FREQ 16000000L /* set to clock frequency in Hz */ #define BAUD_RR ((CPU_FREQ/(16L*BAUD) - 1)) //Compass Directions for Hallway #define North_H 192 // These contants vary with time/location, even after a recalib #define South_H 72 #define East_H 245 #define West_H 135 /*Port Assignments /********************************************************** PORTA - LCD PORTB - NOTHING PORTC - NOTHING (USED TO BE ENCODER CNT FROM ATMEGA 8) PORTD- PIN0,1 - SDL,SCA, PIN4- IC1- RIGHT- CHA PORTE- PIN7- IC3- LEFT- CHA PORTF- NOTHING PORTG- NOTHING ************************************************************ */ float LEFT_SP, LEFT_P_TERM, LEFT_I_TERM, LEFT_D_TERM, RIGHT_SP, RIGHT_P_TERM, RIGHT_I_TERM, RIGHT_D_TERM; signed int LEFT_ERROR, RIGHT_ERROR, ERROR_I; signed int LEFT_VALUE, RIGHT_VALUE, TRAJ_L, TRAJ_R, PREV_LEFT_ERROR, PREV_RIGHT_ERROR; uint8_t LEFT_ENC_VALUE, RIGHT_ENC_VALUE; float Left_Motor_Comp, Right_Motor_Comp; float LEFT_Kp = .35, RIGHT_Kp = .35, LEFT_Ki= 0, RIGHT_Ki = 0, LEFT_Kd= 0, RIGHT_Kd = 0; //10 value arrays for integral int LEFT_I_ERROR[] = {0,0,0,0,0,0,0,0,0,0}; long int Left_I_Sum = 0; int RIGHT_I_ERROR[] = {0,0,0,0,0,0,0,0,0,0}; long int Right_I_Sum = 0; int I_ERROR[] = {0,0,0,0,0,0,0,0,0,0}; int BIAS = 0; long int I_Sum = 0; int i = 0; // global pointer for all integrals int L_I_point = 0, R_I_point = 0, I_point = 0; signed int RIGHT_SPEED, LEFT_SPEED; int PORTD_READ, OLD_L_SPEED, OLD_R_SPEED, LEFT_DIR, OLD_L_DIR, NEW_RIGHT_SPEED, NEW_LEFT_SPEED, SET_SPEED_L, SET_SPEED_R; int RIGHT_DIR, OLD_R_DIR, LEFT_ACCEL, OLD_L_ACCEL, RIGHT_ACCEL, OLD_R_ACCEL, OLD_R_ENC, OLD_L_ENC, NEW_R_ENC, NEW_L_ENC; long LEFT_CNT, RIGHT_CNT; uint16_t LEFT_ENC_CNT, RIGHT_ENC_CNT, LEFT_ENC_CNT2, RIGHT_ENC_CNT2; uint8_t sensor_cnt, new_data, motor_flag, cmd, old_cmd, SBC_CMD, Door_Flag, Remote_Flag; volatile unsigned char snr_num=0,snr_ping=0, snr_rd,snr_avg = 0; volatile signed int TR,TL,CR,CL,BR,BL,BK,TC; volatile unsigned char cr_thresh_s = 18, cl_thresh_s = 18, br_thresh = 20, bl_thresh = 20, bk_thresh = 20, tl_thresh = 12 , tr_thresh = 12, cr_thresh_t = 16, cl_thresh_t = 16, bl_thresh_l = 15, bl_thresh_h = 22, br_thresh_l = 15, br_thresh_h = 22; unsigned char tl_door = 11, tr_door = 6; volatile uint16_t ms_count; uint8_t sensor_cnt; uint8_t Left_Clear, Right_Clear, Wall_Follow = 0; double tl_hall_u, tl_hall_l, tr_hall_u, tr_hall_l; //************************ Sleep ********************************************* void ms_sleep(uint16_t ms) // Sleep function. Sleeps for ms milliseconds. { TCNT1 = 0; ms_count = 0; while (ms_count != ms) ; } /***********************************Steering Code**********************************************/ // cmd1: Turn Left // cmd2: Turn Right // cmd3: Veer Left // cmd4: Veer Right // cmd5: Stop // cmd6: Back up // cmd7: Straight // cmd8: Veer Left Fast // cmd9: Veer Right Fast // cmd10: Veer Left Slow // cmd11: Veer Right Slow // motor_flag = 0: Straight // motor_flag = 1: Stop, Left, Right // motor_flag = 2: Back up // motor_flag = 3: Veer Left, Veer Right, Veer Left Fast, Veer Right Fast void Update_Motors(void) // Updates the motors with the current speed, direction, and accel { //Make sure motors are w/in limits if(LEFT_SPEED > 65) { LEFT_SPEED = 65; } else if(LEFT_SPEED < 30) { LEFT_SPEED = 30; } if(RIGHT_SPEED > 65) { RIGHT_SPEED = 65; } else if(RIGHT_SPEED < 30) { RIGHT_SPEED = 30; } /**************Update Motor Drivers****************/ /******************Left Motor**********************/ //Set Speed and Direction // if(LEFT_SPEED != OLD_L_SPEED || LEFT_DIR != OLD_L_DIR) // Always need to set direction after setting new speed // { I2C_START_TX(LEFT_MOTOR); i2c_transmit(SPEED); i2c_transmit(LEFT_SPEED); i2c_stop(); OLD_L_SPEED = LEFT_SPEED; I2C_START_TX(LEFT_MOTOR); i2c_transmit(COMMAND); i2c_transmit(LEFT_DIR); i2c_stop(); OLD_L_DIR = LEFT_DIR; // } //Set Acceleration if(LEFT_ACCEL != OLD_L_ACCEL) { I2C_START_TX(LEFT_MOTOR); i2c_transmit(ACCEL); i2c_transmit(LEFT_ACCEL); i2c_stop(); OLD_L_ACCEL = LEFT_ACCEL; } /******************Right Motor******************/ //Set Speed and Direction // if(RIGHT_SPEED != OLD_R_SPEED || RIGHT_DIR != OLD_R_DIR) // Always need to set direction after setting new speed // { I2C_START_TX(RIGHT_MOTOR); i2c_transmit(SPEED); i2c_transmit(RIGHT_SPEED); i2c_stop(); OLD_R_SPEED = RIGHT_SPEED; I2C_START_TX(RIGHT_MOTOR); i2c_transmit(COMMAND); i2c_transmit(RIGHT_DIR); i2c_stop(); OLD_R_DIR = RIGHT_DIR; // } //Set Acceleration if(RIGHT_ACCEL != OLD_R_ACCEL) { I2C_START_TX(RIGHT_MOTOR); i2c_transmit(ACCEL); i2c_transmit(RIGHT_ACCEL); i2c_stop(); OLD_R_ACCEL = RIGHT_ACCEL; } } void Stop_Motors(void) // Stops motors instantly, does not ramp down. { LEFT_SPEED = 0; LEFT_DIR = STOP; RIGHT_SPEED = 0; RIGHT_DIR = STOP; SET_SPEED_L = 0; SET_SPEED_R = 0; motor_flag = 1; old_cmd = cmd; cmd = 5; Update_Motors(); ms_sleep(10); } void Stop_Motors2(void) // Stops motors instantly, does not ramp down. // Does not change the RIGHT_SPEED/LEFT_SPEED, RIGHT_DIR/LEFT_DIR constants { // if(cmd != 5) // If not already stopped // { I2C_START_TX(LEFT_MOTOR); i2c_transmit(SPEED); i2c_transmit(0); i2c_stop(); I2C_START_TX(LEFT_MOTOR); i2c_transmit(COMMAND); i2c_transmit(STOP); i2c_stop(); I2C_START_TX(RIGHT_MOTOR); i2c_transmit(SPEED); i2c_transmit(0); i2c_stop(); I2C_START_TX(RIGHT_MOTOR); i2c_transmit(COMMAND); i2c_transmit(STOP); i2c_stop(); motor_flag = 1; old_cmd = cmd; cmd = 5; ms_sleep(10); // } } void Turn_Left(void) { // if(cmd != 1) // If not previously moving left // { if(LEFT_DIR == FORWARD) { Stop_Motors2(); } LEFT_SPEED = 35; LEFT_DIR = REVERSE; RIGHT_SPEED = 35; RIGHT_DIR = FORWARD; SET_SPEED_L = 35; SET_SPEED_R = 35; motor_flag = 1; old_cmd = cmd; cmd = 1; // } } void Turn_Right(void) { // if(cmd != 2) // If not previously moving right // { if(RIGHT_DIR == FORWARD) { Stop_Motors2(); } LEFT_SPEED = 35; LEFT_DIR = FORWARD; RIGHT_SPEED = 35; RIGHT_DIR = REVERSE; SET_SPEED_L = 35; SET_SPEED_R = 35; motor_flag = 1; old_cmd = cmd; cmd = 2; // } } void Veer_Left_S(void) // Veers left slowly { if(motor_flag == 2 || motor_flag == 1) //Robot was moving in reverse or turning { Stop_Motors2(); } if(cmd != 10) // If not already veering left { LEFT_SPEED = 40; LEFT_DIR = FORWARD; RIGHT_SPEED = 42; RIGHT_DIR = FORWARD; SET_SPEED_L = 40; SET_SPEED_R = 42; motor_flag = 3; old_cmd = cmd; cmd = 10; } } void Veer_Left(void) { if(motor_flag == 2 || motor_flag == 1) //Robot was moving in reverse or turning { Stop_Motors2(); } // if(cmd != 3) // If not already veering left // { LEFT_SPEED = 40; LEFT_DIR = FORWARD; RIGHT_SPEED = 45; RIGHT_DIR = FORWARD; SET_SPEED_L = 40; SET_SPEED_R = 45; motor_flag = 3; old_cmd = cmd; cmd = 3; // } } void Veer_Left_F(void) // Veers left fast { if(motor_flag == 2 || motor_flag == 1) //Robot was moving in reverse or turning { Stop_Motors2(); } if(cmd != 8) // If not already veering left fast { LEFT_SPEED = 40; LEFT_DIR = FORWARD; RIGHT_SPEED = 50; RIGHT_DIR = FORWARD; SET_SPEED_L = 40; SET_SPEED_R = 50; motor_flag = 3; old_cmd = cmd; cmd = 8; } } void Veer_Right_S(void) // Veers right slowly { if(motor_flag == 2 || motor_flag == 1) //Robot was moving in reverse or turning { Stop_Motors2(); } if(cmd != 11) // If not already veering right { LEFT_SPEED = 45; LEFT_DIR = FORWARD; RIGHT_SPEED = 40; RIGHT_DIR = FORWARD; SET_SPEED_L = 45; SET_SPEED_R = 40; motor_flag = 3; old_cmd = cmd; cmd = 11; } } void Veer_Right(void) { if(motor_flag == 2 || motor_flag == 1) //Robot was moving in reverse or turning { Stop_Motors2(); } // if(cmd != 4) // If not already veering right // { LEFT_SPEED = 50; LEFT_DIR = FORWARD; RIGHT_SPEED = 40; RIGHT_DIR = FORWARD; SET_SPEED_L = 50; SET_SPEED_R = 40; motor_flag = 3; old_cmd = cmd; cmd = 4; // } } void Veer_Right_F(void) // Veers right fast { if(motor_flag == 2 || motor_flag == 1) //Robot was moving in reverse or turning { Stop_Motors2(); } if(cmd != 9) // If not already veering right fast { LEFT_SPEED = 55; LEFT_DIR = FORWARD; RIGHT_SPEED = 40; RIGHT_DIR = FORWARD; SET_SPEED_L = 55; SET_SPEED_R = 40; motor_flag = 3; old_cmd = cmd; cmd = 9; } } void Back_Up(void) { // if(cmd != 6) // If not already going reverse // { if(LEFT_DIR == FORWARD || RIGHT_DIR == FORWARD) { Stop_Motors2(); } LEFT_SPEED = 45; LEFT_DIR = REVERSE; RIGHT_SPEED = 45; RIGHT_DIR = REVERSE; SET_SPEED_L = 45; SET_SPEED_R = 45; motor_flag = 2; old_cmd = cmd; cmd = 6; // } } void Drive_Straight(void) { if(motor_flag == 2 || motor_flag == 1) //Robot was moving in reverse or turning { Stop_Motors2(); } // if(cmd != 7) // If not already going straight // { LEFT_SPEED = 50; LEFT_DIR = FORWARD; RIGHT_SPEED = 50; RIGHT_DIR = FORWARD; SET_SPEED_L = 50; SET_SPEED_R = 50; motor_flag = 0; old_cmd = cmd; cmd = 7; // } } void Motor_Driver_Init(void) // Initialize motor drivers. Zero speed and Maximum Accel. // Min. accel = 255. Max accel = 0. { LEFT_SPEED = 0; OLD_L_SPEED = 255; LEFT_DIR = STOP; OLD_L_DIR = STOP; LEFT_ACCEL = 0; //max acceleration OLD_L_ACCEL = 255; RIGHT_SPEED = 0; OLD_R_SPEED = 255; RIGHT_DIR = STOP; OLD_R_DIR = STOP; RIGHT_ACCEL = 0; //max acceleration OLD_R_ACCEL = 255; SET_SPEED_L = 0; SET_SPEED_R = 0; } /***************************Serial Code*************************************/ int initialize_uart0(void) // Intitializes UART0 to 9600 baud rate { /* enable UART0 */ UBRR0H = (BAUD_RR >> 8) & 0xff; UBRR0L = BAUD_RR & 0xff; UCSR0B = _BV(TXEN)|_BV(RXEN)|_BV(RXCIE0); //enable transmitter, receiver and receiver interrupt return 0; } int uart0_send_character(char ch) // Sends a character through UART0 { /* output character to UART0 */ while ((UCSR0A & _BV(UDRE)) == 0) ; UDR0 = ch; return ch; } int uart0_send_integer(int var) // Sends a integer through UART0 { /* output interger to UART0 */ while ((UCSR0A & _BV(UDRE)) == 0) ; UDR0 = var; return var; } void Send_Sensor_Data(void) // Sends sensor data to the Single Board Computer using UART0 { uart0_send_integer(0xFF); uart0_send_integer(BR); //Send outer right data uart0_send_integer(BL); //Send outer left data uart0_send_integer(CR); //Send center right data uart0_send_integer(CL); //Send center left data uart0_send_integer(TL); //Send top left data uart0_send_integer(TR); //Send top right data uart0_send_integer(BK); //Send back sonar data uart0_send_integer(TC); //Send back sonar data uart0_send_integer(bearing_8(1)); //Send compass data } void Send_Motor_Data(void) // Sends motor data to the Single Board Computer using UART0 { uart0_send_integer(0xFF); if(LEFT_DIR == FORWARD) { uart0_send_character('F'); //Send left motor direction } else if(LEFT_DIR == REVERSE) { uart0_send_character('R'); //Send left motor direction } if(RIGHT_DIR == FORWARD) { uart0_send_character('F'); //Send right motor direction } else if(RIGHT_DIR == REVERSE) { uart0_send_character('R'); //Send right motor direction } } /**************************Encoder******************************************/ void INIT_ENCODER(void) // Sets up Input Capture 1 and 3 for rising edge to read encoder ticks // Enables a IC interupt for each { LEFT_ENC_VALUE = 0; //Start both encoder counts at 0 RIGHT_ENC_VALUE = 0; TIFR |= _BV(ICF1); // Clears ICP1 Flag ETIFR |= _BV(ICF3); // Clears ICP3 Flag TCCR1B |= _BV(ICES1); // Rising Edge will trigger ICP1 TCCR3B |= _BV(ICES3); // Rising Edge will trigger ICP3 TIMSK |= _BV(TICIE1); // Enables ICP1 Interupt ETIMSK |= _BV(TICIE3); // Enables ICP3 Interupt TCNT1 = 0; TCNT3 = 0; } void Start_Distance_S(void) // Starts an encoder counter for distance measurement while going straight or veering { RIGHT_ENC_CNT = 0; LEFT_ENC_CNT = 0; } double Get_Distance_S(void) // Returns the distance traveled in inches while going straight or veering { double right_dist; double left_dist; right_dist = RIGHT_ENC_CNT*(8*3.14)/416; // Distance right wheel has traveled. 416 cnt/rev, circum = pi*8 left_dist = LEFT_ENC_CNT*(8*3.14)/416; return right_dist/2 + left_dist/2; // Returns the average of the two measurements } void Start_Distance_T(void) // Starts an encoder counter for distance measurement while turning { RIGHT_ENC_CNT2 = 0; LEFT_ENC_CNT2 = 0; } double Get_Distance_T(void) // Returns the distance traveled in inches while turning { double right_dist2; double left_dist2; right_dist2 = RIGHT_ENC_CNT2*(8*3.14)/416; // Distance right wheel has traveled. 416 cnt/rev, circum = pi*8 left_dist2 = LEFT_ENC_CNT2*(8*3.14)/416; return right_dist2/2 + left_dist2/2; // Returns the average of the two measurements } /************************* Behaviors **************************************************/ void Read_Sensors(void) // Reads in the sonars. Can only ping one sonar at a time. // Right now the fastest pinging is only 1.5 pings/sec. Tried changing range, gain, and placing in a interrupt. { if (snr_ping==0) // If not already pinging { srf08_select_unit(0xE0+snr_num); // Selects the sonar to read start_ping(SRF08_INCHES); // Starts ranging in units of inches snr_ping=1; } else { if (check_ping()==1) // If sonar is finished pinging { snr_rd=read_ping(); // Reads in the distance measurement if (snr_rd!=0) { switch(0xE0+snr_num) // Determines which sonar is currently being read { case SNR_TR: // Read top right sonar TR=snr_rd; break; case SNR_TL: // Read top left sonar TL=snr_rd; break; case SNR_CR: // Read center right sonar CR=snr_rd; break; case SNR_CL: // Read center left sonar CL=snr_rd; break; case SNR_BR: // Read bottom right sonar BR=snr_rd; break; case SNR_BL: // Read bottom left sonar BL=snr_rd; break; case SNR_BK: // Read back sonar BK=snr_rd; break; case SNR_TC: // Read top center sonar TC=snr_rd; break; } // Ends switch } // Ends (snr_rd!=0) snr_ping=0; snr_num=snr_num+2; if(snr_num==16) { snr_num=0; } } // End if(check_ping()==1) } // End else } void Right_90(void) // Turns right 90 degrees { Start_Distance_T(); Turn_Right(); while(Get_Distance_T() <= 14) // Breaks once it has turned a quarter cirlce. circum = 20*pi. Overturns a bit. { Update_Motors(); } Stop_Motors(); } void Left_90(void) // Turns left 90 degrees { Start_Distance_T(); Turn_Left(); while(Get_Distance_T() <= 14) // Breaks once it has turned a quarter cirlce. circum = 20*pi. Overturns a bit. { Update_Motors(); } Stop_Motors(); } void Decision_Eq(void) // Obstacle avoids based on sonar readings. { Left_Clear = 0; // Flags to tell if the left/right sides are clear of objects Right_Clear = 0; // ****************Right Motors************************************** if(TC > 12) // If nothing in front of Top Center Sonar { if(CL <= 22) // In front { if(CL <= 14 && BL > 22) // Directly in Front { if(RIGHT_DIR == FORWARD) { Stop_Motors2(); } RIGHT_SPEED = 65 - 2.5*(CL); // 2.5 and 12 RIGHT_DIR = REVERSE; SET_SPEED_R = RIGHT_SPEED; } else if(CL <= 14 && BL <= 22) // Directly in Front or Front Left { if(RIGHT_DIR == FORWARD) { Stop_Motors2(); } RIGHT_SPEED = 65 - 2.5*(.1*BL + .9*CL); RIGHT_DIR = REVERSE; SET_SPEED_R = RIGHT_SPEED; } else if(CL > 14 && BL <= 22) // Front left { if(RIGHT_DIR == REVERSE) { Stop_Motors2(); } RIGHT_SPEED = 55 + 2*((.4*BL + .6*CL) - 22); // 1.8 good RIGHT_DIR = FORWARD; SET_SPEED_R = RIGHT_SPEED; } else if(CL > 14 && BL > 22) // Front but far { if(RIGHT_DIR == REVERSE) { Stop_Motors2(); } RIGHT_SPEED = 55 + 1.8*(CL - 22); // 1.8 good RIGHT_DIR = FORWARD; SET_SPEED_R = RIGHT_SPEED; } } // Ends if(CL <= 22) else if(BL <= 12 && CL > 22) // To the side { if(RIGHT_DIR == REVERSE) { Stop_Motors2(); } RIGHT_SPEED = 55 + 1.5*(BL - 12); // 1.5 is good RIGHT_DIR = FORWARD; SET_SPEED_R = RIGHT_SPEED; } else if(BL > 12 && CL > 22) // Nothing close { if(Wall_Follow == 0) // If not wall following { if(RIGHT_DIR == REVERSE) { Stop_Motors2(); } RIGHT_SPEED = 55; RIGHT_DIR = FORWARD; SET_SPEED_R = RIGHT_SPEED; } Right_Clear = 1; } // ****************Left Motors************************************** if(CR <= 22) // In front { if(CR <= 14 && BR > 22) // Directly in Front { if(LEFT_DIR == FORWARD) { Stop_Motors2(); } LEFT_SPEED = 65 - 2.5*(CR); LEFT_DIR = REVERSE; SET_SPEED_L = LEFT_SPEED; } else if(CR <= 14 && BR <= 22) // Directly in Front or Front Right { if(LEFT_DIR == FORWARD) { Stop_Motors2(); } LEFT_SPEED = 65 - 2.5*(.1*BR + .9*CR); LEFT_DIR = REVERSE; SET_SPEED_L = LEFT_SPEED; } else if(CR > 14 && BR <= 22) // Front Right { if(LEFT_DIR == REVERSE) { Stop_Motors2(); } LEFT_SPEED = 55 + 2*((.4*BR + .6*CR) - 22); LEFT_DIR = FORWARD; SET_SPEED_L = LEFT_SPEED; } else if(CR > 14 && BR > 22) // Front but far { if(LEFT_DIR == REVERSE) { Stop_Motors2(); } LEFT_SPEED = 55 + 1.8*(CR - 22); LEFT_DIR = FORWARD; SET_SPEED_L = LEFT_SPEED; } } // Ends if(CR <= 22) else if(BR <= 12 && CR > 22) // To the side { if(LEFT_DIR == REVERSE) { Stop_Motors2(); } LEFT_SPEED = 55 + 1.5*(BR - 12); LEFT_DIR = FORWARD; SET_SPEED_L = LEFT_SPEED; } else if(BR > 12 && CR > 22) // Nothing close { if(Wall_Follow == 0) // If not wall following { if(LEFT_DIR == REVERSE) { Stop_Motors2(); } LEFT_SPEED = 55; LEFT_DIR = FORWARD; SET_SPEED_L = LEFT_SPEED; } Left_Clear = 1; } } // Ends if(TC > 12) // ****** Top Center Sonar************************************************** else if(TC <= 12) // 10 is pretty good { if(LEFT_DIR == FORWARD || RIGHT_DIR == FORWARD) // If not previously going in reverse { Stop_Motors2(); } LEFT_SPEED = 52 - TC; LEFT_DIR = REVERSE; SET_SPEED_L = LEFT_SPEED; RIGHT_SPEED = 52 - TC; RIGHT_DIR = REVERSE; SET_SPEED_R = RIGHT_SPEED; } // ****** Back Sonar******************************************************** if(RIGHT_DIR == REVERSE && LEFT_DIR == REVERSE) // Checks if going in reverse { if(BK <= 15) // Stop if back sonar too close to object. 20 too high. 15 too low. { // lcd_clr(); // printf("BK < 15"); Stop_Motors(); Left_Clear = 0; // Koolio is not clear of objects Right_Clear = 0; } } } void Navigate_Door(void) // Navigates through the door. /* TL CL CR TR 0 0 0 0 Straight 0 0 0 1 Veer Left X X 1 X Turn Left X 1 X X Turn Right 1 0 0 0 Veer Right 1 0 0 1 Straight */ { // unsigned char tl_door = 11, tr_door = 6; cr_thresh_s = 12, cl_thresh_s = 12; if((TL > tl_door) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (TR > tr_door)){ //not close to anything. 0000 //DRIVE_CMD = 7; // Drive Straight if(Door_Flag != 1) { Drive_Straight(); Door_Flag = 1; } } else if((TL > tl_door) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(TR > tr_door)) { //object on right. 0001 //DRIVE_CMD = 3; // Veer Left if(Door_Flag != 2) { Veer_Left(); Door_Flag = 2; } } else if(!(CR > cr_thresh_s)) { //object at front right. XX1X //DRIVE_CMD = 5; // Stop //Stop_Motors(); if(Door_Flag != 3) { Turn_Left(); Door_Flag = 3; } } else if(!(CL > cl_thresh_s)) { //object on front left. X1XX //DRIVE_CMD = 5; // Stop //Stop_Motors(); if(Door_Flag != 4) { Turn_Right(); Door_Flag = 4; } } else if(!(TL > tl_door) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (TR > tr_door)) { //object on left. 1000 //DRIVE_CMD = 4; // Veer Right if(Door_Flag != 5) { Veer_Right(); Door_Flag = 5; } } else if(!(TL > tl_door) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(TR > tr_door)) { //objects on either side. 1001 //DRIVE_CMD = 7; // Drive Straight if(Door_Flag != 6) { Drive_Straight(); Door_Flag = 6; } } } void Guide_Hallway_Left(char head) // Wall follows left wall and obstacle avoids { tl_hall_u = 40, tl_hall_l = 22, tr_hall_u = 55, tr_hall_l = 40; // 45,22 Decision_Eq(); if((Left_Clear == 1) && (Right_Clear == 1)) // If Koolio is clear of objects { //*********** Right Motors************************************** if(TL >= tl_hall_u) // Too far from left wall { if(RIGHT_DIR == REVERSE) { Stop_Motors2(); } RIGHT_SPEED = 46 + 0.3*(TL - tl_hall_u); // 0.3 maybe RIGHT_DIR = FORWARD; if(RIGHT_SPEED > 50) { RIGHT_SPEED = 50; } SET_SPEED_R = RIGHT_SPEED; } else if(TL <= tl_hall_l) // Too close to left wall { if(RIGHT_DIR == REVERSE) { Stop_Motors2(); } RIGHT_SPEED = 44 + 0.3*(TL - tl_hall_l); if(RIGHT_SPEED < 40) { RIGHT_SPEED = 40; } RIGHT_DIR = FORWARD; SET_SPEED_R = RIGHT_SPEED; } else if((TL < tl_hall_u) && (TL > tl_hall_l)) // Within the bounds { if(RIGHT_DIR == REVERSE) { Stop_Motors2(); } if(head == 'W') { RIGHT_SPEED = 45 + 1*(bearing_8(1) - West_H); // 1 maybe. 1.5 too high } else if(head == 'E') { if(bearing_8(1) > North_H) // 192 - 255 { RIGHT_SPEED = 45 + 1*(bearing_8(1) - East_H); // 1 maybe. 1.5 too high } else // 0 - 192 { RIGHT_SPEED = 55; // 1 maybe. 1.5 too high } } if(RIGHT_SPEED > 55) { RIGHT_SPEED = 55; } RIGHT_DIR = FORWARD; SET_SPEED_R = RIGHT_SPEED; } //*********** Left Motors*************************************************************** if(LEFT_DIR == REVERSE) { Stop_Motors2(); } LEFT_SPEED = 45; LEFT_DIR = FORWARD; SET_SPEED_L = LEFT_SPEED; /* if(TR >= tr_hall_u) // Too far from right wall { if(LEFT_DIR == REVERSE) { Stop_Motors(); } LEFT_SPEED = 46 + 0.5*(TR - tr_hall_u); LEFT_DIR = FORWARD; if(LEFT_SPEED > 55) { LEFT_SPEED = 55; } SET_SPEED_L = LEFT_SPEED; } else if(TR <= tr_hall_l) // Too close to the right wall { if(LEFT_DIR == REVERSE) { Stop_Motors(); } LEFT_SPEED = 44 + 0.5*(TR - tr_hall_l); if(LEFT_SPEED < 35) { LEFT_SPEED = 35; } LEFT_DIR = FORWARD; SET_SPEED_L = LEFT_SPEED; } else if((TR < tr_hall_u) && (TR > tr_hall_l)) // Within the bounds { if(LEFT_DIR == REVERSE) { Stop_Motors(); } LEFT_SPEED = 45; LEFT_DIR = FORWARD; SET_SPEED_L = LEFT_SPEED; } */ } // Ends if((Left_Clear == 1) && (Right_Clear == 1)) } void Guide_Hallway_Right(char head) // Wall follows right wall and obstacle avoids { tr_hall_u = 40, tr_hall_l = 22, tl_hall_u = 55, tl_hall_l = 40; Decision_Eq(); if((Left_Clear == 1) && (Right_Clear == 1)) // If Koolio is clear of objects { //*********** Left Motors*************************************************************** if(TR >= tr_hall_u) // Too far from Right wall { if(LEFT_DIR == REVERSE) { Stop_Motors2(); } LEFT_SPEED = 46 + 0.3*(TR - tr_hall_u); LEFT_DIR = FORWARD; if(LEFT_SPEED > 50) { LEFT_SPEED = 50; } SET_SPEED_L = LEFT_SPEED; } else if(TR <= tr_hall_l) // Too close to Right wall { if(LEFT_DIR == REVERSE) { Stop_Motors2(); } LEFT_SPEED = 44 + 0.3*(TR - tr_hall_l); LEFT_DIR = FORWARD; if(LEFT_SPEED < 40) { LEFT_SPEED = 40; } SET_SPEED_L = LEFT_SPEED; } else if((TR < tr_hall_u) && (TR > tr_hall_l)) // Within the bounds { if(LEFT_DIR == REVERSE) { Stop_Motors2(); } if(head == 'E') { if(bearing_8(1) < South_H) { //LEFT_SPEED = 45 - 1.5*((255 - East) + bearing_8(1)); // 0.7 maybe LEFT_SPEED = 35; } else { LEFT_SPEED = 45 + 1*(East_H - bearing_8(1)); // 0.7 maybe } } else if(head == 'W') { LEFT_SPEED = 45 + 1*(West_H - bearing_8(1)); // 0.7 maybe } LEFT_DIR = FORWARD; SET_SPEED_L = LEFT_SPEED; } //*********** Right Motors*************************************************************** if(RIGHT_DIR == REVERSE) { Stop_Motors2(); } RIGHT_SPEED = 45; RIGHT_DIR = FORWARD; SET_SPEED_R = RIGHT_SPEED; /* if(TL > tl_hall_u) // Too far from Left wall { if(RIGHT_DIR == REVERSE) { Stop_Motors(); } RIGHT_SPEED = 65 - 2*(75 - TL); RIGHT_DIR = FORWARD; if(RIGHT_SPEED > 65 || TL > 75) { RIGHT_SPEED = 65; } SET_SPEED_R = RIGHT_SPEED; } else if(TL < tl_hall_l) // Too close to the Left wall { if(RIGHT_DIR == FORWARD) { Stop_Motors(); } RIGHT_SPEED = 2*(75 - TL); if(RIGHT_SPEED > 65 || TL > 75) { RIGHT_SPEED = 65; } RIGHT_DIR = REVERSE; SET_SPEED_R = RIGHT_SPEED; } else if((TL <= tl_hall_u) && (TL >= tl_hall_l)) // Within the bounds { if(RIGHT_DIR == REVERSE) { Stop_Motors(); } RIGHT_SPEED = 55; RIGHT_DIR = FORWARD; SET_SPEED_R = RIGHT_SPEED; } */ } // Ends if((Left_Clear == 1) && (Right_Clear == 1)) } void Sonar_BL(void) // Reads BL sonar { srf08_select_unit(SNR_BL); // Selects the sonar to read start_ping(SRF08_INCHES); // Starts ranging in units of inches while(check_ping() != 1); // Waits untill pinging is finished BL=read_ping(); } void Sonar_TL(void) // Reads TL sonar { srf08_select_unit(SNR_TL); // Selects the sonar to read start_ping(SRF08_INCHES); // Starts ranging in units of inches while(check_ping() != 1); // Waits untill pinging is finished TL=read_ping(); } void Sonar_TR(void) // Reads TR sonar { srf08_select_unit(SNR_TR); // Selects the sonar to read start_ping(SRF08_INCHES); // Starts ranging in units of inches while(check_ping() != 1); // Waits untill pinging is finished TR=read_ping(); } void Sonar_BR(void) // Reads BR sonar { srf08_select_unit(SNR_BR); // Selects the sonar to read start_ping(SRF08_INCHES); // Starts ranging in units of inches while(check_ping() != 1); // Waits untill pinging is finished BR=read_ping(); } void Sonar_BK(void) // Reads BK sonar { srf08_select_unit(SNR_BK); // Selects the sonar to read start_ping(SRF08_INCHES); // Starts ranging in units of inches while(check_ping() != 1); // Waits untill pinging is finished BK=read_ping(); } void Sonar_CR(void) // Reads CR sonar { srf08_select_unit(SNR_CR); // Selects the sonar to read start_ping(SRF08_INCHES); // Starts ranging in units of inches while(check_ping() != 1); // Waits untill pinging is finished CR=read_ping(); } void Sonar_CL(void) // Reads BR sonar { srf08_select_unit(SNR_CL); // Selects the sonar to read start_ping(SRF08_INCHES); // Starts ranging in units of inches while(check_ping() != 1); // Waits untill pinging is finished CL=read_ping(); } void Face_Door(void) // Make sure Koolio is facing the door { Sonar_BL(); Sonar_BR(); if(BL >= (BR + 2)) // Angled from the right { Turn_Right(); Update_Motors(); while(BL >= (BR + 2)) { Sonar_BL(); Sonar_BR(); } Stop_Motors(); } else if(BR >= (BL + 2)) { Turn_Left(); Update_Motors(); while(BR >= (BL + 2)) { Sonar_BL(); Sonar_BR(); } Stop_Motors(); } } void Arbiter_Lab(char direction) // Behavior Arbiter for moving in the lab. // Executes obstacle avoidance and make sure Koolio is in the correct compass heading. { Wall_Follow = 0; // Flag to signal that koolio is not wall following Decision_Eq(); // If driving straight/stopped, // Make sure traveling in the correct heading if((RIGHT_DIR == STOP && LEFT_DIR == STOP) || (RIGHT_DIR == FORWARD && LEFT_DIR == FORWARD)) { if(direction == 'W') { Turn_West(); } else if(direction == 'E') { Turn_East(); } else if(direction == 'N') { Turn_North(); } else if(direction == 'S') { Turn_South(); } } } void Arbiter_Hall(char wall, char heading) // Behavior Arbiter for moving in the hall following a left/right wall. // Executes obstacle avoidance and make sure Koolio is in the correct compass heading. { Wall_Follow = 1; // Flag to signal that koolio is wall following if(wall == 'L') { if(heading == 'W') { Guide_Hallway_Left('W'); } else if(heading == 'E') { Guide_Hallway_Left('E'); } } else if(wall == 'R') { if(heading == 'W') { Guide_Hallway_Right('W'); } else if(heading == 'E') { Guide_Hallway_Right('E'); } } // If driving straight/stopped, make sure traveling in the correct heading if((RIGHT_DIR == STOP && LEFT_DIR == STOP) || (RIGHT_DIR == FORWARD && LEFT_DIR == FORWARD)) { if(heading == 'W') { Turn_West(); } else if(heading == 'E') { Turn_East(); } } } void Navigate_Lab_Out(void) // Navigates Koolio through the lab. Goes from a start point to the lab door while obstacle avoiding. { Turn_West_Perfect(); Start_Distance_S(); while(Get_Distance_S() <= 155 || TR <= 35) // While encoder distance is not met and Top Right sonar is reading // less than 35 inches. Actual distance to cabinet is 130 inches. // Used to be 140 { Read_Sensors(); Arbiter_Lab('W'); Update_Motors(); // Update motors and read in sensors } Stop_Motors(); ms_sleep(1000); lcd_clr(); printf("Turning Right"); Turn_North_Perfect(); // Turn North works better then Turning 90 degrees Start_Distance_S(); // while(Get_Distance_S() <= 165) // Actual distance is 180. used to be 160. // while(TL <= (tl_thresh + 15) || TR <= (tr_thresh + 15)) // While in doorway while(Get_Distance_S() <= 275 || (TL <= (tl_thresh + 15) || TR <= (tr_thresh + 15))) // Actual distance is 280. From turn point to hallway { Read_Sensors(); Arbiter_Lab('N'); Update_Motors(); // Update motors and read in sensors } Stop_Motors(); /* lcd_clr(); printf("Done w 2"); // ms_sleep(3000); // Turn_North_Perfect(); // ms_sleep(2000); while(TL >= tl_thresh) // While not near doorway { Read_Sensors(); Arbiter_N(); Update_Motors(); // Update motors and read in sensors } Stop_Motors(); Update_Motors(); lcd_clr(); printf("In Door"); // ms_sleep(3000); tl_door = 11, tr_door = 6; while(TL <= (tl_thresh + 15) || TR <= (tr_thresh + 15)) // While in doorway { Read_Sensors(); Navigate_Door(); Update_Motors(); // Update motors and read in sensors } Stop_Motors(); Update_Motors(); */ lcd_clr(); // Koolio is out of the lab door printf("Out of Door"); ms_sleep(2000); } void Navigate_Lab_In(void) // Navigates Koolio through the lab. Goes from the lab entrace back to the start point while obstacle avoiding. { Sonar_TL(); Sonar_TR(); tl_door = 6, tr_door = 11, tl_thresh = 14, tr_thresh = 14; // 12,12 Door_Flag = 0; while(TL >= tl_thresh || TR >= tr_thresh) // While not near doorway { Read_Sensors(); Navigate_Door(); Update_Motors(); // Update motors and read in sensors } lcd_clr(); printf("in door"); Door_Flag = 0; while(TR <= (tr_thresh + 8)) // While in doorway { Read_Sensors(); Navigate_Door(); Update_Motors(); // Update motors and read in sensors } lcd_clr(); printf("out of door"); Start_Distance_S(); while(Get_Distance_S() <= 195 || TL <= 30) // Actual distance is 180. 185 too short. 190 may be too short. // 28 may be too low { Read_Sensors(); Arbiter_Lab('S'); Update_Motors(); // Update motors } Stop_Motors(); ms_sleep(2000); lcd_clr(); printf("Turning Left"); Turn_East_Perfect(); Start_Distance_S(); while(Get_Distance_S() <= 175 || TL <= 40) // While encoder distance is not met and Top Right sonar is reading // less than 50 inches. Actual distance to cabinet is 130 inches. // 170 is too low. { Read_Sensors(); Arbiter_Lab('E'); Update_Motors(); // Update motors and read in sensors } Stop_Motors(); lcd_clr(); printf("Done"); } void Navigate_To_Schwartz(void) // Navigates Koolio from the lab entrace to Schwartz office. Wall follows and obstacle avoids. { Turn_West_Perfect(); Start_Distance_S(); while(Get_Distance_S() <= 314) // Go to Schwartz Office. Actual distance is 306" from mid door to mid door. { Read_Sensors(); Arbiter_Hall('L','W'); Update_Motors(); // Update motors } Stop_Motors(); lcd_clr(); printf("Done w 5"); ms_sleep(3000); Turn_South_Perfect(); // Face Schwartz door // Face_Door(); // Does not work very well lcd_clr(); printf("Facing Door"); ms_sleep(2000); //============= Enter/Leave Schwartz Office ================================== /* Sonar_TL(); Sonar_TR(); tl_door = 6, tr_door = 6, tl_thresh = 14, tr_thresh = 14; // 6,11 Door_Flag = 0; while(TL >= tl_thresh || TR >= tr_thresh) // While not near doorway { Read_Sensors(); Navigate_Door(); Update_Motors(); // Update motors and read in sensors } lcd_clr(); printf("in door"); Door_Flag = 0; while(TR <= (tr_thresh + 8)) // While in doorway { Read_Sensors(); Navigate_Door(); Update_Motors(); // Update motors and read in sensors } lcd_clr(); printf("out of door"); Start_Distance_S(); while(Get_Distance_S() <= 155) // Actual distance is 156" { Read_Sensors(); Arbiter_Lab('S'); Update_Motors(); // Update motors } Stop_Motors(); ms_sleep(2000); lcd_clr(); printf("turn around"); Turn_North_Perfect(); Start_Distance_S(); while((Get_Distance_S() <= 260) || (TL <= (tl_thresh + 15) || TR <= (tr_thresh + 15))) // Actual distance is 252" { Read_Sensors(); Arbiter_Lab('N'); Update_Motors(); // Update motors } Stop_Motors(); lcd_clr(); printf("out of door"); ms_sleep(2000); */ } void Navigate_To_Arroyo(void) // Navigates Koolio from the lab entrace to Arroyo office. Wall follows and obstacle avoids. { Turn_East_Perfect(); Start_Distance_S(); while(Get_Distance_S() <= 1090) // Go to Arroyo Office. Actual distance is 1070" from mid door to mid door. { Read_Sensors(); Arbiter_Hall('L','E'); Update_Motors(); // Update motors } Stop_Motors(); lcd_clr(); printf("Done w 5"); ms_sleep(3000); Turn_North_Perfect(); // Face Arroyo door // Face_Door(); // Does not work very well lcd_clr(); printf("Facing Door"); ms_sleep(2000); } void Navigate_From_Schwartz(void) // Navigates from Schwartz office back to lab entrace. Wall follows and obstacle avoids. { lcd_clr(); printf("Backwards"); Turn_East_Perfect(); // Go back to lab. Start_Distance_S(); while(Get_Distance_S() <= 312) // Go back to lab. Actual distance is 306" from mid door to mid door. { Read_Sensors(); Arbiter_Hall('R','E'); Update_Motors(); // Update motors } Stop_Motors(); lcd_clr(); printf("At door"); ms_sleep(2000); Turn_South_Perfect(); // Face entrance to lab // Face_Door(); // Does not work very well lcd_clr(); printf("Facing door"); ms_sleep(2000); } void Navigate_From_Arroyo(void) // Navigates from Arroyo office back to lab entrace. Wall follows and obstacle avoids. { lcd_clr(); printf("Backwards"); Turn_West_Perfect(); // Go back to lab Start_Distance_S(); while(Get_Distance_S() <= 1095) // Go back to lab. Actual distance is 1070" from mid door to mid door. { Read_Sensors(); Arbiter_Hall('L','W'); Update_Motors(); // Update motors } Stop_Motors(); lcd_clr(); printf("At door"); ms_sleep(2000); Turn_South_Perfect(); // Face entrance to lab // Face_Door(); // Does not work very well lcd_clr(); printf("Facing door"); ms_sleep(2000); } /************************** PID ************************************************/ void PID_loop(void) // PID loop for motor control. Only currently using a P term. // New Speed = Kp*P-term, and P-term = Enc_Val(Desired) - Enc_Val(Actual). // I approximated a linear relationship between Speed vs. Encoder Counts. // Speed = 2.85*encoder_cnt + 13. Note that Speed is the speed being sent to // the motor controller and not the actual speed of the motors. // // LEFT_ENC_VALUE/RIGHT_ENC_VALUE is the amount of encoder ticks per 0.1 seconds. // SET_SPEED_L/SET_SPEED_R is the desired speed to be sent to the motor controller. // TRAJ_L/TRAJ_R is what the encoder value should be if going at the desired speed. // LEFT_SPEED/RIGHT_SPEED is the current speed that the motor controller is using. { //lcd_clr(); //printf("R:%i,%i,L:%i,%i ",RIGHT_ENC_VALUE,RIGHT_SPEED,LEFT_ENC_VALUE,LEFT_SPEED); LEFT_VALUE = LEFT_ENC_VALUE; RIGHT_VALUE = RIGHT_ENC_VALUE; TRAJ_L = (SET_SPEED_L - 13)/2.85; // Using that Speed = 2.85*encoder_count + 13 TRAJ_R = (SET_SPEED_R - 13)/2.85; // Using that Speed = 2.85*encoder_count + 13 if(SET_SPEED_L < 13) { TRAJ_L = 0; } if(SET_SPEED_R < 13) { TRAJ_R = 0; } //Calculate Proportional Terms LEFT_ERROR = TRAJ_L - LEFT_VALUE; // Calculate P term in terms of the encoder count error LEFT_P_TERM = LEFT_Kp*(float)LEFT_ERROR; RIGHT_ERROR = TRAJ_R - RIGHT_VALUE; RIGHT_P_TERM = RIGHT_Kp*(float)RIGHT_ERROR; Left_Motor_Comp = (LEFT_P_TERM)*2.94; // Converting to terms of speed Right_Motor_Comp = (RIGHT_P_TERM)*2.81; // Converting to terms of speed NEW_LEFT_SPEED = (LEFT_SPEED + Left_Motor_Comp); NEW_RIGHT_SPEED = (RIGHT_SPEED + Right_Motor_Comp); //make sure motors are w/in limits if(NEW_LEFT_SPEED > 65) { LEFT_SPEED = 65; } else if(NEW_LEFT_SPEED < 30) { LEFT_SPEED = 30; } else { LEFT_SPEED = NEW_LEFT_SPEED; } if(NEW_RIGHT_SPEED > 65) { RIGHT_SPEED = 65; } else if(NEW_RIGHT_SPEED < 30) { RIGHT_SPEED = 30; } else { RIGHT_SPEED = NEW_RIGHT_SPEED; } // Update_Motors(); This does not work here } //*************** Interupt Service Routines****************************************************** SIGNAL(SIG_OUTPUT_COMPARE1A) // Used for ms_sleep { ms_count++; } SIGNAL(SIG_OUTPUT_COMPARE2) // Run PID loop every 0.1 secs { if(sensor_cnt == 0) { LEFT_ENC_VALUE = 0; // Zeros the encoder tick counter RIGHT_ENC_VALUE = 0; } sensor_cnt++; if(sensor_cnt == 10) // If been 0.1 sec { sensor_cnt = 0; PID_loop(); // Calls the pid loop every .1 seconds } } SIGNAL(SIG_UART0_RECV) //UART0 receive interrupt service routine { new_data = UDR0; // Save rxed data from SBC. } SIGNAL(SIG_INPUT_CAPTURE1) // Used to read in the right encoder counts. Every falling edge generates an interupt. // Increments a counter for PID loop, a counter for the distance traveled, and a counter for turning. { RIGHT_ENC_VALUE++; // Counter for PID control if(RIGHT_DIR == FORWARD && LEFT_DIR == FORWARD) // If going straight or veering { RIGHT_ENC_CNT++; // Counter for distance traveled } if(cmd == 1 || cmd == 2) // If turning left or right { RIGHT_ENC_CNT2++; } } SIGNAL(SIG_INPUT_CAPTURE3) // Used to read in the right encoder counts. Every falling edge generates an interupt. // Increments a counter for PID loop, a counter for the distance traveled, and a counter for turning. { LEFT_ENC_VALUE++; // Counter for PID control if(RIGHT_DIR == FORWARD && LEFT_DIR == FORWARD) // If going straight or veering { LEFT_ENC_CNT++; // Counter for distance traveled } if(cmd == 1 || cmd == 2) // If turning left or right { LEFT_ENC_CNT2++; } } /***************************** Main Code **************************************************/ int main(void) { // Set data direction for ports DDRA = 0xFF; // PortA is LCD. Set DDR for outputs. //PORTC = 0x00; // Encoder count from ATMEGA8. No longer using ATMEGA8. //DDRC = 0x00; //PORTB = 0x00; // Nothing. May need it for relay card control //DDRB = 0xFF; //PORTE = 0x00; // Pin 7- Left- IC3. Set DDR for inputs. //DDRE = 0x00; //PORTD = 0x00; // Pin 4- Right- IC1 //DDRD = 0x00; // Set all pins as inputs //Initialize motor drivers Motor_Driver_Init(); /* initialize stdio */ fdevopen(def_putc, NULL, 0); //initialize LCD lcd_init(); lcd_clr(); printf("Initialized"); // INIT_TIMER3(); // Initialize timer for reading in sensors INIT_TIMER2(); // Initialize timer for PID loop INIT_TIMER1(); // Initialize timer for sleep // INIT_TIMER0(); // Initialize timer updating motors and reading in sensors INIT_ENCODER(); // Intialize input captures for encoders initialize_uart0(); //Initialize UART0 for communication with embedded board (SBC) //Set variables sensor_cnt = 0; motor_flag = 0; old_cmd = 0; cmd = 0; SBC_CMD = 0; Door_Flag = 0; new_data = 0; Remote_Flag = 0; sei(); //Enable interrupts Stop_Motors(); // Stop Motors ms_sleep(3000); /* //************************ Delivery Mode (w/o SBC) *************************** while(1) { SBC_CMD = 2; // Go to Schwartz office Navigate_Lab_Out(); // Navigates out of lab if(SBC_CMD == 1) // Go to Arroyo office command from SBC { Navigate_To_Arroyo(); // Navigates to Schwartz office Navigate_From_Arroyo(); // Navigates back to lab entrace } else if(SBC_CMD == 2) // Go to Schwartz office command from SBC { Navigate_To_Schwartz(); // Navigates to Schwartz office Navigate_From_Schwartz(); // Navigates back to lab entrace } Navigate_Lab_In(); // Navigates into lab while(1); } */ /* //************************ Delivery Mode ************************************ lcd_clr(); printf("Delivery Mode"); while(1) { //Set variables sensor_cnt = 0; motor_flag = 0; old_cmd = 0; cmd = 0; SBC_CMD = 0; Door_Flag = 0; new_data = 0; while(new_data == 0); // While no commands sent from SBC. Valid commands are 1 - 7. lcd_clr(); printf("%i",new_data); SBC_CMD = new_data; // Temp variable to hold the command sent by the SBC. Navigate_Lab_Out(); // Navigates out of lab if(SBC_CMD == 1) // Go to Arroyo office command from SBC { Navigate_To_Arroyo(); // Navigates to Schwartz office Navigate_From_Arroyo(); // Navigates back to lab entrace } else if(SBC_CMD == 2) // Go to Schwartz office command from SBC { Navigate_To_Schwartz(); // Navigates to Schwartz office Navigate_From_Schwartz(); // Navigates back to lab entrace } Navigate_Lab_In(); // Navigates into lab } // Ends main while(1) */ //************************ Remote Mode ************************************ // This mode can be used for the AI control and for the Remote control from the website while(1) { lcd_clr(); // Displays the data received from the SBC printf("SBC CMD: %i); Read_Sensors(); // Read in sensor data Send_Sensor_Data(); // Send sensor data to SBC /* while(new_data == 0) // Obstalce avoid untill get a remote move command { Read_Sensors(); Decision_Eq(); Update_Motors(); } */ if(new_data == 0) // Stop motors if no data has been received by SBC { Stop_Motors(); } if(new_data == 3) // Left { if(Remote_Flag != 3) { Turn_Left(); Remote_Flag = 3; } } else if(new_data == 4) // Right { if(Remote_Flag != 4) { Turn_Right(); Remote_Flag = 4; } } else if(new_data == 5) // Back Up { if(Remote_Flag != 5) { Back_Up(); Remote_Flag = 5; } } else if(new_data == 6) // Straight { if(Remote_Flag != 6) { Drive_Straight(); Remote_Flag = 6; } } else if(new_data == 7) // Stop { if(Remote_Flag != 7) { Stop_Motors(); Remote_Flag = 7; } } Update_Motors(); } // End of Main while(1) } // End of main code //TESTING=================================================================================================================== //========================================================================================================================= /* while(1) { // Read_Sensors(); // Arbiter_Lab('E'); // Update_Motors(); lcd_clr(); printf("%i",bearing_8(1)); ms_sleep(500); } /* Turn_West_Perfect(); while(1) { Read_Sensors(); Decision_Eq(); Update_Motors(); lcd_clr(); printf("%i,%i,%i,%i,%i",BL,CL,CR,BR,BK); } */ /* srf08_select_unit(SNR_BL); // Selects the sonar to read srf08_set_range(140); // Set range for 6000 mm srf08_select_unit(SNR_CL); // Selects the sonar to read srf08_set_range(140); // Set range for 6000 mm srf08_select_unit(SNR_CR); // Selects the sonar to read srf08_set_range(140); // Set range for 6000 mm srf08_select_unit(SNR_BR); // Selects the sonar to read srf08_set_range(140); // Set range for 6000 mm srf08_select_unit(SNR_BK); // Selects the sonar to read srf08_set_range(140); // Set range for 6000 mm srf08_select_unit(SNR_TL); // Selects the sonar to read srf08_set_range(140); // Set range for 6000 mm srf08_select_unit(SNR_TR); // Selects the sonar to read srf08_set_range(140); // Set range for 6000 mm srf08_select_unit(SNR_BL); // Selects the sonar to read srf08_set_gain(25); // Set range for 6000 mm srf08_select_unit(SNR_CL); // Selects the sonar to read srf08_set_range(25); // Set range for 6000 mm srf08_select_unit(SNR_CR); // Selects the sonar to read srf08_set_range(25); // Set range for 6000 mm srf08_select_unit(SNR_BR); // Selects the sonar to read srf08_set_range(25); // Set range for 6000 mm srf08_select_unit(SNR_BK); // Selects the sonar to read srf08_set_range(25); // Set range for 6000 mm srf08_select_unit(SNR_TL); // Selects the sonar to read srf08_set_range(25); // Set range for 6000 mm srf08_select_unit(SNR_TR); // Selects the sonar to read srf08_set_range(25); // Set range for 6000 mm */ /* Drive_Straight(); Update(); Start_Distance(); // printf("hello"); while(Get_Distance() <= 10) { Update(); } Stop_Motors(); Update(); while(1); */ /* Drive_Straight(); start_distance(); ms_sleep(6000); printf("dist:%i",get_distance()); while(1); */ /* Drive_Straight(); while(1) { lcd_clr(); printf("R:%i,%i,L:%i,%i ",RIGHT_ENC_VALUE,RIGHT_SPEED,LEFT_ENC_VALUE,LEFT_SPEED); LEFT_ENC_VALUE = 0; RIGHT_ENC_VALUE = 0; ms_sleep(100); PID_loop(); } */ /* //Calculate Set Points ms_sleep(100); //wait for intial encoder values LEFT_VALUE = LEFT_ENC_VALUE; RIGHT_VALUE = RIGHT_ENC_VALUE; TRAJ = ((LEFT_VALUE / 2) + (RIGHT_VALUE / 2)); PREV_LEFT_ERROR = TRAJ - LEFT_VALUE; PREV_RIGHT_ERROR = TRAJ - RIGHT_VALUE; */ /* //while((cmd == old_cmd) && (motor_flag == 0)) //only enter PID loop if going straight while(1) // Go thru PID loop every 0.1 sec { //lcd_clr(); //printf("%u", cmd); //Send_sensor_data(); BIAS = 0; PREV_LEFT_ERROR = 0; PREV_RIGHT_ERROR = 0; lcd_clr(); printf("R:%i,%i,L:%i,%i ",RIGHT_ENC_VALUE,RIGHT_SPEED,LEFT_ENC_VALUE,LEFT_SPEED); LEFT_ENC_VALUE = 0; RIGHT_ENC_VALUE = 0; ms_sleep(100); LEFT_VALUE = LEFT_ENC_VALUE; RIGHT_VALUE = RIGHT_ENC_VALUE; //TRAJ = ((LEFT_VALUE / 2) + (RIGHT_VALUE / 2)); TRAJ = (SET_SPEED - 13)/2.85; // Using that Speed = 2.85*encoder_count + 13 //TRAJ = 20; // Calculate Integral Terms ERROR_I = LEFT_VALUE - RIGHT_VALUE + BIAS; // Integral term is speed difference b/t motors. Bias is for turning, ie Bias = 0 if going straight. I_ERROR[I_point] = ERROR_I; I_Sum = 0; for(i = 0; i < 10; i++) { I_Sum += I_ERROR[i]; } LEFT_I_TERM = LEFT_Ki*(float)I_Sum; RIGHT_I_TERM = RIGHT_Ki*(float)I_Sum; if(I_point == 9) { I_point = 0; } else { I_point++; } //Calculate Proportional Terms LEFT_ERROR = TRAJ - LEFT_VALUE - LEFT_I_TERM; // Calculate P term LEFT_P_TERM = LEFT_Kp*(float)LEFT_ERROR; RIGHT_ERROR = TRAJ - RIGHT_VALUE + RIGHT_I_TERM; RIGHT_P_TERM = RIGHT_Kp*(float)RIGHT_ERROR; /* //Calculate Integral Terms LEFT_I_ERROR[L_I_point] = LEFT_ERROR; Left_I_Sum = 0; for(i = 0; i < 10;i++) { Left_I_Sum += LEFT_I_ERROR[i]; } RIGHT_I_ERROR[R_I_point] = RIGHT_ERROR; Right_I_Sum = 0; for(i = 0; i < 10;i++) { Right_I_Sum += RIGHT_I_ERROR[i]; } */ /* //Calculate Derivative Terms //LEFT_D_TERM = LEFT_Kd*(LEFT_ERROR - PREV_LEFT_ERROR); LEFT_D_TERM = LEFT_Kd*(LEFT_ERROR + PREV_LEFT_ERROR); PREV_LEFT_ERROR = LEFT_ERROR; //RIGHT_D_TERM = RIGHT_Kd*(RIGHT_ERROR - PREV_RIGHT_ERROR); RIGHT_D_TERM = RIGHT_Kd*(RIGHT_ERROR + PREV_RIGHT_ERROR); PREV_RIGHT_ERROR = RIGHT_ERROR; */ /* Left_Motor_Comp = ((LEFT_P_TERM + LEFT_I_TERM + LEFT_D_TERM)*.4); Right_Motor_Comp = ((RIGHT_P_TERM + RIGHT_I_TERM + RIGHT_D_TERM)*.4); Left_Motor_Comp = (LEFT_P_TERM+LEFT_D_TERM+LEFT_I_TERM)*2.94; // Converting to terms of speed Right_Motor_Comp = (RIGHT_P_TERM+RIGHT_D_TERM+RIGHT_I_TERM)*2.81; // Converting to terms of speed */ /* Left_Motor_Comp = (LEFT_P_TERM)*2.94; // Converting to terms of speed Right_Motor_Comp = (RIGHT_P_TERM)*2.81; // Converting to terms of speed NEW_LEFT_SPEED = (LEFT_SPEED + Left_Motor_Comp); NEW_RIGHT_SPEED = (RIGHT_SPEED + Right_Motor_Comp); /* if(L_I_point == 9) { L_I_point = 0; } else { L_I_point++; } if(R_I_point == 9) { R_I_point = 0; } else { R_I_point++; } */ /* if(abs(NEW_LEFT_SPEED - NEW_RIGHT_SPEED) > 0) //2 { //make sure motors are w/in limits if(NEW_LEFT_SPEED > 255) { LEFT_SPEED = 255; } else if(NEW_LEFT_SPEED < 0) { LEFT_SPEED = 0; } else { LEFT_SPEED = NEW_LEFT_SPEED; } if(NEW_RIGHT_SPEED > 255) { RIGHT_SPEED = 255; } else if(NEW_RIGHT_SPEED < 0) { RIGHT_SPEED = 0; } else { RIGHT_SPEED = NEW_RIGHT_SPEED; } } } //end of PID loop */ /*======================================================================================================= OLD CRAP: /* void Guide_Hallway_Left(void) // Wall follows left wall and obstacle avoids /* 0 is > threshold: BLCLCRBRBKTLTR Action 0 0 0 0 X 0 0 Straight 0 0 0 0 X 0 1 Veer Left 0 0 0 0 X 1 0 Veer Right 0 0 0 0 X 1 1 Straight 0 0 0 1 X X X Veer Left X 0 1 X X X X Turn Left X 1 0 X X X X Turn Right X 1 1 X 0 X X Back Up X 1 1 X 1 X X Stop (for now) 1 0 0 0 X X X Veer Right 1 0 0 1 X X X Straight */ /* { unsigned char tl_hall_u = 15, tl_hall_l = 10; if((BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh_l) && (TL > tl_hall_l) && (TL < tl_hall_u)){ //not close to anything. 0000X //DRIVE_CMD = 7; // Drive Straight Drive_Straight(); } else if((BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh_l) && (TL > tl_hall_l) && (TL > tl_hall_u)){ //too far from left wall. 000001 Veer_Left_S(); } else if((BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh_l) && (TL < tl_hall_l) && (TL < tl_hall_u)){ //not close to left wall. 000010 Veer_Right_S(); } else if((BL > bl_thresh) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(BR > br_thresh_l)) { //object on right. 0001X //DRIVE_CMD = 3; // Veer Left Veer_Left_F(); } else if((BL > bl_thresh) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(BR > br_thresh_h)) { //object on right. 0001X //DRIVE_CMD = 3; // Veer Left Veer_Left(); } else if((CL > cl_thresh_s) && !(CR > cr_thresh_t)) { //object at front right. X01XX //DRIVE_CMD = 1; // Turn Left Turn_Left(); } else if(!(CL > cl_thresh_t) && (CR > cr_thresh_s)) { //object on front left. X10XX //DRIVE_CMD = 2; // Turn Right Turn_Right(); } else if(!(CL > cl_thresh_s) && !(CR > cr_thresh_s) && (BK > bk_thresh)){ //object ahead. X11X0 //DRIVE_CMD = 6; // Back Up Back_Up(); } else if(!(CL > cl_thresh_s) && !(CR > cr_thresh_s) && !(BK > bk_thresh)){ //object ahead and behind. X11X1 //DRIVE_CMD = 5; // Stop Stop_Motors(); } else if(!(BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh)) { //object on left. 1000X //DRIVE_CMD = 4; // Veer Right Veer_Right_F(); } else if(!(BL > bl_thresh_h) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh)) { //object on left. 1000X //DRIVE_CMD = 4; // Veer Right Veer_Right(); } else if(!(BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(BR > br_thresh_l)) { //objects on either side. 1001X //DRIVE_CMD = 7; // Drive Straight Drive_Straight(); } } void Guide_Hallway_Right(void) // Wall follows right wall and obstacle avoids /* 0 is > threshold: BLCLCRBRBKTLTR Action 0 0 0 0 X 0 0 Straight 0 0 0 0 X 0 1 Veer Left 0 0 0 0 X 1 0 Veer Right 0 0 0 0 X 1 1 Straight 0 0 0 1 X X X Veer Left X 0 1 X X X X Turn Left X 1 0 X X X X Turn Right X 1 1 X 0 X X Back Up X 1 1 X 1 X X Stop (for now) 1 0 0 0 X X X Veer Right 1 0 0 1 X X X Straight */ /* { unsigned char tr_hall_u = 15, tr_hall_l = 10;// tr_hall = 70; if((BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh_l) && (TR > tr_hall_l) && (TR < tr_hall_u)){ //not close to anything. 0000X //DRIVE_CMD = 7; // Drive Straight Drive_Straight(); } else if((BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh_l) && (TR > tr_hall_l) && (TR > tr_hall_u)){ //too far from right wall. 000001 Veer_Right_S(); } else if((BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh_l) && (TR < tr_hall_l) && (TR < tr_hall_u)){ //not close to right wall. 000010 Veer_Left_S(); } else if((BL > bl_thresh) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(BR > br_thresh_l)) { //object on right. 0001X //DRIVE_CMD = 3; // Veer Left Veer_Left_F(); } else if((BL > bl_thresh) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(BR > br_thresh_h)) { //object on right. 0001X //DRIVE_CMD = 3; // Veer Left Veer_Left(); } else if((CL > cl_thresh_s) && !(CR > cr_thresh_t)) { //object at front right. X01XX //DRIVE_CMD = 1; // Turn Left Turn_Left(); } else if(!(CL > cl_thresh_t) && (CR > cr_thresh_s)) { //object on front left. X10XX //DRIVE_CMD = 2; // Turn Right Turn_Right(); } else if(!(CL > cl_thresh_s) && !(CR > cr_thresh_s) && (BK > bk_thresh)){ //object ahead. X11X0 //DRIVE_CMD = 6; // Back Up Back_Up(); } else if(!(CL > cl_thresh_s) && !(CR > cr_thresh_s) && !(BK > bk_thresh)){ //object ahead and behind. X11X1 //DRIVE_CMD = 5; // Stop Stop_Motors(); } else if(!(BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh)) { //object on left. 1000X //DRIVE_CMD = 4; // Veer Right Veer_Right_F(); } else if(!(BL > bl_thresh_h) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh)) { //object on left. 1000X //DRIVE_CMD = 4; // Veer Right Veer_Right(); } else if(!(BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(BR > br_thresh_l)) { //objects on either side. 1001X //DRIVE_CMD = 7; // Drive Straight Drive_Straight(); } } //Based on the current readings from the sensors, decide what course of //action to take. I.e., veer left, go straight, etc. //At the end of this function, the variable DRIVE_CMD will be set to the //code of the recommended heading. /* Heading codes: CMD# COMMAND 1 Turn left 2 Turn right 3 Veer left 4 Veer right 5 Stop motors 6 Back up 7 Drive straight 0 is > threshold: BLCLCRBRBK Action X 0 1 X X Turn Left X 1 0 X X Turn Right X 1 1 X 0 Back Up X 1 1 X 1 Stop (for now) */ /* void set_heading() { if((BL > bl_thresh) && (CL > cl_thresh) && (CR > cr_thresh) && (BR > br_thresh)){ //not close to anything. 0000X DRIVE_CMD = 7; // Drive Straight } else if((BL > bl_thresh) && (CL > cl_thresh) && (CR > cr_thresh) && !(BR > br_thresh)) { //object on right. 0001X DRIVE_CMD = 3; // Veer Left } else if((CL > cl_thresh) && !(CR > cr_thresh)) { //object at front right. X01XX DRIVE_CMD = 1; // Turn Left } else if(!(CL > cl_thresh) && (CR > cr_thresh)) { //object on front left. X10XX DRIVE_CMD = 2; // Turn Right } else if(!(CL > cl_thresh) && !(CR > cr_thresh) && (BK > bk_thresh)){ //object ahead. X11X0 DRIVE_CMD = 6; // Back Up } else if(!(CL > cl_thresh) && !(CR > cr_thresh) && !(BK > bk_thresh)){ //object ahead and behind. X11X1 DRIVE_CMD = 5; // Stop } else if(!(BL > bl_thresh) && (CL > cl_thresh) && (CR > cr_thresh) && (BR > br_thresh)) { //object on left. 1000X DRIVE_CMD = 4; // Veer Right } else if(!(BL > bl_thresh) && (CL > cl_thresh) && (CR > cr_thresh) && !(BR > br_thresh)) { //objects on either side. 1001X DRIVE_CMD = 7; // Drive Straight } } void Decision(void) // Determines how to move based on commands from embedded computer { int new_data_flag = 0; //Steering code //read serial command from embedded board //new_data is command read from embedded board if(new_data == 1) { if((new_data_flag ==1)) { Turn_Left(); } new_data_flag = 1; } if(new_data == 2) { if(new_data_flag == 2) { Turn_Right(); } new_data_flag = 2; } if(new_data == 3) { if(new_data_flag == 3) { Veer_Left(); } new_data_flag = 3; } if(new_data == 4) { if(new_data_flag == 4) { Veer_Right(); } new_data_flag = 4; } if(new_data == 5) { if(new_data_flag == 5) { Stop_Motors(); } new_data_flag = 5; } if(new_data == 6) { if(new_data_flag == 6) { Back_Up(); } new_data_flag = 6; } if(new_data == 7) { if(new_data_flag == 7) { Drive_Straight(); } new_data_flag = 7; } } void Decision(void) 0 is > threshold: BLCLCRBRBK Action 0 0 0 0 X Straight 0 0 0 1 X Veer Left X 0 1 X X Turn Left X 1 0 X X Turn Right X 1 1 X 0 Back Up X 1 1 X 1 Stop (for now) 1 0 0 0 X Veer Right 1 0 0 1 X Straight { if((BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh_l)){ //not close to anything. 0000X //DRIVE_CMD = 7; // Drive Straight Drive_Straight(); } else if((BL > bl_thresh) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(BR > br_thresh_l)) { //object on right. 0001X //DRIVE_CMD = 3; // Veer Left Veer_Left_F(); } else if((BL > bl_thresh) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(BR > br_thresh_h)) { //object on right. 0001X //DRIVE_CMD = 3; // Veer Left Veer_Left(); } else if((CL > cl_thresh_s) && !(CR > cr_thresh_t)) { //object at front right. X01XX //DRIVE_CMD = 1; // Turn Left Turn_Left(); } else if(!(CL > cl_thresh_t) && (CR > cr_thresh_s)) { //object on front left. X10XX //DRIVE_CMD = 2; // Turn Right Turn_Right(); } else if(!(CL > cl_thresh_s) && !(CR > cr_thresh_s) && (BK > bk_thresh)){ //object ahead. X11X0 //DRIVE_CMD = 6; // Back Up Back_Up(); } else if(!(CL > cl_thresh_s) && !(CR > cr_thresh_s) && !(BK > bk_thresh)){ //object ahead and behind. X11X1 //DRIVE_CMD = 5; // Stop Stop_Motors(); } else if(!(BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh)) { //object on left. 1000X //DRIVE_CMD = 4; // Veer Right Veer_Right_F(); } else if(!(BL > bl_thresh_h) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && (BR > br_thresh)) { //object on left. 1000X //DRIVE_CMD = 4; // Veer Right Veer_Right(); } else if(!(BL > bl_thresh_l) && (CL > cl_thresh_s) && (CR > cr_thresh_s) && !(BR > br_thresh_l)) { //objects on either side. 1001X //DRIVE_CMD = 7; // Drive Straight Drive_Straight(); } } */