/* Linefol.c Line follower robot control program. This has the additional code for finding the line and avoiding running into things. This program is compiled using the Linux shareware C2C compiler available at: http://www.geocities.com/SiliconValley/Network/3656/ The output .asm code is assembled with the Linux freeware assembler gpasm avaiable at: http://reality.sgi.com/jamesb/gpasm/ MPASM can also be used. **Note: I had to make some small modifications to gpasm so it would work with the 16C73A chip. For reference here is the makefile for this project -------------------------------------------------------- #Linefol.c make file all: linefol.hex # Assemble the program from the asm source linefol.hex: linefol.asm gpasm -p 16c73 linefol.asm # # Generate the asm source file from the C source . linefol.asm: linefol.c c2c -vs0x20 -Ip16c73a.inc -olinefol.asm linefol.c # # # Clean up the directory clean: rm -f linefol.hex core linefol.asm linefol.lst # #PIC programmer command pgm: tm4 -c 16c73 -p /dev/ttyS1 -z 3ffa linefol.hex ----------------------------------------------------------- */ #define FOSC 8000000 #pragma CLOCK_FREQ 8000000 #include "16c74.h" //#define TMR0DIV 256-(FOSC/(16000*16)) //Compiler will not do this math! //Divider constant for 16,000 interrupts per second //Timer 0 is running at 500K counts/sec #define TMR0DIV 226 #define BAUD 2400 //#define BAUDDIV (FOSC/(BAUD*64)) - 1 #define BAUDDIV 51 /* these values written into ADCCON0 set the A-D channel mux address and converison clock to 1/32 osc. This makes a 32uS converson time*/ #define AD_LMFB 0x99 /* channel 3 */ #define AD_RMFB 0x91 /* channel 2 */ #define AD_SPEED 0x81 /* channel 0 */ #define AD_TRACK 0x89 /* channel 1 */ #define AD_LINEFIND 0xa1 /* ch 4 */ #define AD_LEFTDETECT 0xa9 /* ch 5 */ #define AD_RIGHTDETECT 0xb1 /* ch 6 */ #define AD_BATTERY 0xb9 /* ch 7 */ /* Analog inputs */ char speed; char track; char rmfb; char lmfb; char line_find; char left_detect; char right_detect; char battery; char right_speed,right_prm; char left_speed,left_prm; char bot_mode; #define MODE_STOP 0 #define MODE_FINDLINE 1 #define MODE_CENTER 2 #define MODE_ROTATE_L 3 #define MODE_ROTATE_R 4 #define MODE_TRACKLINE_RAMP_UP 5 #define MODE_TRACKLINE 6 #define MODE_FREE_ROTATE_R 0x24 #define MODE_BACK_OUT 0x25 #define MODE_ROTATE_BACK_OUT 0x26 #define MODE_BACKUP_AND_TURN_AROUND 0x27 #define MODE_BACKUP_TO_LINE 0x28 #define MODE_BACK_OUT_RIGHT 0x29 #define MODE_BACK_OUT_LEFT 0x2a #define FWD1 0xe2 #define FWD2 0xd8 #define FWD3 0xc4 #define FWD4 0xb0 #define REV1 30 #define REV2 40 #define REV3 60 #define REV4 80 char prescale; char timer; char prm_ctl; /* bits in prm_ctl */ #define R_fwd 0 #define L_fwd 1 char workB; #define FwdLeft 7 /*Left Motor pulse forward*/ #define RevLeft 6 /*Left Motor pulse reverse*/ #define FwdRight 5 /*Right Motor pulse forward */ #define RevRight 4 /*Right MOtor pulse reverse */ #define Bump_R 3 /*Right bump sensor */ #define IR_R 2 /*Right IR sensor */ #define IR_L 1 /*Left IR sensor */ #define Bump_L 0 /*Left bump sensor */ #define P_width 11 /*Motor pulse width in interrupt ticks*/ char r_speed; char l_speed; char speed_to_pulse_rate(char speed); char crcH,crcL; char tx_ptr,rx_ptr,txstate,rxstate; char c_timer,line_correlation,peak_correlation,limit_timer; char mode_bits; char cor[8]@0xa0; /* 8 byte correlator for line finding */ char tx_buf[34]@0xa8; /* Tx serial port buffer */ char rx_buf[34]@0xca; /* Rx serial port buffer */ #define COUNT 1 /* offset to count field in tx/rx buffers */ #define ADDRESS 0 /* offset to address field in tx/rx buffers */ #define DATA 2 /* offset to start of data in tx/rx buffers */ #define DIR 0 char test_hi@0xfe; char detect_line(); //------------------------------------------------------------------ /* Interrupts from Timer 0 occur at about 16,000 per second. */ void interrupt() { char R_pulse,L_pulse; static char divide10; if ((INTCON & (1 << T0IF))) { //Make sure it's a TMR0 interrupt! clear_bit(INTCON,T0IF); //Clear timer 0 interrupt flag TMR0 = TMR0DIV + TMR0; //restart timer 0 if(prescale-- == 0){ set_bit(PORTC,0); //timing test bit set prescale = 160; if(timer != 0) timer--; //10 ms pertick timer if(c_timer != 0) c_timer--; clear_bit(PORTC,0); //Timing test bit if(divide10++ >= 10){ if(limit_timer != 0) limit_timer--; //100ms per tick timer divide10 = 0; } } PORTB = workB; //output the prm motor control bits to port B workB = 0; /* adjust right pulse rate modulator */ if (right_prm < P_width) { if (prm_ctl & (1 << R_fwd)) set_bit(workB,FwdRight); else set_bit(workB,RevRight); } if (right_prm-- == 0) { right_prm = speed_to_pulse_rate(right_speed) + P_width; if (right_speed > 127) clear_bit(prm_ctl,R_fwd); else set_bit(prm_ctl,R_fwd); } /*adjust left pulse rate modulator */ if (left_prm < P_width) { if (prm_ctl & (1 << L_fwd)) set_bit(workB,FwdLeft); else set_bit(workB,RevLeft); } if (left_prm-- == 0) { left_prm = speed_to_pulse_rate(left_speed) + P_width; if (left_speed > 127) clear_bit(prm_ctl,L_fwd); else set_bit(prm_ctl,L_fwd); } } if(bot_mode == 0) workB = 0; } //---------------------------------------------------------------------- /*send character if tx ready and return true else return false if tx not ready */ char tx_byte(char c){ if (PIR1 & (1 << TXIF)) { TXREG = c; return 1; } else return 0; } //----------------------------------------------------------------------- /* compute 16 bit CRC on 1 data byte. crcH and crcL are global variables which accumulate the 16 bit CRC. They must be cleared before starting to compute 16 bit CRC on a string. The final output value will be in crcH and crcL. */ void crc(char data){ char i; short crc16; for (i=0;i<8;i++) { //Do all 8 bits in the data byte if (data & 0x80) crcH ^= 0x80; //if data bit is 1 then flip crc msb data = data << 1; // get next data bit if (crcH & 0x80) { //crc msb is 1 asm{ bcf STATUS,C; rlf _crcL; //16 bit left shift rlf _crcH; movlw 80h; xorwf _crcH,F; //xor with 0x8005 movlw 5; xorwf _crcL,F; } } else { //crc msb is 0 asm{ bcf STATUS,C; rlf _crcL; //16 bit left shift rlf _crcH; } } } } //----------------------------------------------------------------------- void set_tx_buffer_crc(){ int j = tx_buf[COUNT + 0] + 2; //get data size int i; crcH = 0; //Clear crc16 crcL = 0; for (i=0;i 127) { //See if speed is negative ! return speed & 0x7f; } else { return speed ^ 0x7f; } } //---------------------------------------------------------------------- char ad_convert(char channel) { char x; set_bit(PORTC,1); //while (INTCON & (1 << GIE)) disable_interrupt(GIE); ADCON0 = channel; for (x=8;x>0;x--); //wait at least 20 us for data to stabilize //enable_interrupt(GIE); //interrupts on again set_bit(ADCON0,GO_DONE); //start A-D conversion while (ADCON0 & (1 << NOT_DONE)); //wait for conversion to complete clear_bit(PORTC,1); return ADRES+128; //Return signed result } //--------------------------------------------------------------------- /* Saturating addition. Result is clipped at +127 or -128 instead of overflowing. Note the parameters a and b and signed char values (-128..+127) but the compiler thinks they are unsigned values (0..255) so I had to do some strange looking things below. */ char addsat(char a, char b) { char r; if (a > 127) goto a_minus; //See if a is minus if (b > 127) goto a_b_diff; //a is plus, check b for minusness r = a + b; if (r > 127) r = 127; //clip at +127 return r; a_minus: if (b < 0x80) goto a_b_diff; r = a + b; if (r < 0x80) r = 0x80; //clip at -128 return r; a_b_diff: return a + b; //No clipping needed } //--------------------------------------------------------------------- /* return the absolute value of x */ char abs(char x){ if(x > 127){ x ^= 0xff; x++; } return x; } //----------------------------------------------------------------------- /* return the negative of x */ char neg(char x){ x ^= 0xff; x++; return x; } //---------------------------------------------------------------------- /* return x if its 0 or less else return 0 */ char clip_pos(char x){ if(x < 128) x = 0; return x; } //---------------------------------------------------------------------- /* return x if it's 0 or higher else return 0 */ char clip_neg(char x){ if(x > 127) x = 0; return x; } //--------------------------------------------------------------------- char motors_stopped(){ if((abs(rmfb) < 8) && (abs(lmfb < 8))) return 1; else return 0; } //---------------------------------------------------------------------- char integrate(char speed,char mfb,char integ) { speed = speed + 128; //convert to unsigned mfb = mfb + 128; if (speed > mfb) { integ++; if (integ == 0x80) integ--; //Clip at +127 return integ; } else { integ--; if (integ == 0x7f) integ++; //Clip at -128 return integ; } } //--------------------------------------------------------------------- void track_line(char speed){ r_speed = addsat(speed,track); l_speed = addsat(speed, 0 - track); } //---------------------------------------------------------------------- char detect_line(){ char i,x; for(i=0;i<7;i++){ //Shift correlator values cor[i] = cor[i+1]; } x = line_find ; x = x >> 1; //divide by 2 if(x > 63) x = x | 0x80;//sign extend if(x > 8) if(x < 128) x = 8; if(x < 0xf8) if(x > 127) x = 0xf8; if(x < 8) x = 0; if(x > 0xf8) x = 0; cor[i] = x; //Stuff x into correlator x = 0; for(i=2;i<4;i++){ //correlate x = x + cor[i]; x = x - cor[i+3]; } if(x > 127) x = 0; //disallow negative values return x; } //---------------------------------------------------------------------- main() { char i; char minus_track; char l_integrate; char r_integrate; short crc16; test_hi = 0; INTCON = 0; STATUS = 0; PORTA = 0; PORTB = 0; PORTC = 0; T1CON = 0; TMR0 = 0; workB = 0; mode_bits = 0; tx_ptr = 0; rx_ptr = 0; peak_correlation = 0; line_correlation = 0; RCSTA = 0x90; //SPEN + CREN (configure serial port) set_bit(STATUS,RP0); //Bank1 set_tris_b(0x0f); //Port B bits 1:2:3:4 are sensor inputs set_tris_c(0xc0); //Port C all outputs except 6:7 are UART pins TRISA = 0xff; //port A all analog inputs TRISD = 0; //port D all outputs for now TRISE = 7; //Port E inputs (analog) OPTION_REG = 0x01; //Prescaler 1:4 assigned to Timer 0, pullups on portB ADCON1 = 0; //Set all port A to analog with VREF=VCC TXSTA = 0xa2; //Set tx usart status/ctrl reg= CSRC + TXEN + TRMT SPBRG = BAUDDIV; //Set baudrate clear_bit(STATUS,RP0); //Bank0 clear_bit(INTCON,T0IF); //Clear Timer 0 interrupt flag TMR0 = TMR0DIV; //Start Timer 0 set_bit(INTCON,T0IE); //Enable Timer 0 interrupts set_bit(INTCON,GIE); //Enable global interrupts bot_mode = MODE_FINDLINE; timer = 20; //200 ms while(timer > 0); //Wait for everything to stabilize timer = 50; c_timer = 3; limit_timer = 0; make_info_pkt(); while (1) { set_bit(PORTC,2); //Set portc bit 0 for timing testing speed = ad_convert(AD_SPEED); //get speed value from speed pot track = ad_convert(AD_TRACK); //get line tracking error from photocells line_find = ad_convert(AD_LINEFIND); //Rear line finding sensor /* unsigned A to D conversion values */ left_detect = ad_convert(AD_LEFTDETECT) + 128; //Left proximity detector right_detect = ad_convert(AD_RIGHTDETECT) + 128; //Right proximity detector battery = ad_convert(AD_BATTERY) + 128; ///Battery voltage, unsigned if(battery < 136){ battery = ad_convert(AD_BATTERY); //Check it again to be sure if(battery < 136){ bot_mode = 0; //Stop motors if Battery < 8 volts !!! PORTB = 0; } } clear_bit(PORTC,2); //clear timing test bit //NOTE: These should be moved into the switch code below... if(bot_mode == MODE_ROTATE_L){ track = clip_neg(track); r_speed = addsat(FWD1,track); l_speed = addsat(REV1, 0 - track); if(timer == 0){ if(motors_stopped()){ bot_mode=MODE_TRACKLINE_RAMP_UP; timer = abs(speed); } } } if(bot_mode == MODE_ROTATE_R){ //Rotate the robot right (clockwise)... track = clip_pos(track); r_speed = addsat(REV1,track); l_speed = addsat(FWD1, 0 - track); if(timer == 0){ //...until front sensors find a line if(motors_stopped()){ bot_mode=MODE_TRACKLINE_RAMP_UP; timer = abs(speed); } if(limit_timer == 0){ //Timeout! give up. bot_mode = MODE_FINDLINE; timer = 50; } } } switch(bot_mode){ case MODE_TRACKLINE_RAMP_UP: //Slowly increase speed track_line(speed + timer); if(timer == 0) bot_mode = MODE_TRACKLINE; break; case MODE_TRACKLINE: //Follow the line... track_line(speed); if(~PORTB & 0xf){ //Turn around if IR or bump sensor sees something bot_mode = MODE_BACKUP_AND_TURN_AROUND; timer = 30; } break; case MODE_FINDLINE: //Find a line and stop on it r_speed = FWD2; // while avoiding objects l_speed = FWD2; i = ~PORTB & 0xf; if(i == 6){ if(TMR0 & 1){ //Random left or right turn after backing up bot_mode = MODE_BACK_OUT_LEFT; }else{ bot_mode = MODE_BACK_OUT_RIGHT; } timer = 50; //determines backup distance }else{ if(i & 2){ //Left IR sensor r_speed = REV2 ; l_speed = FWD1; timer = 20; //Allow motors time to reverse } if(i & 4){ //Right IR sensor l_speed = REV2; r_speed = FWD1; timer = 20; //Allow motors time to reverse } if(i & 8){ //Right bump sensor input set_bit(mode_bits,DIR); //left turn after backing up bot_mode = MODE_BACK_OUT; timer = 80; //This determines how far we back up } if(i & 1){ //Left bump sensor input clear_bit(mode_bits,DIR); //right turn after backing up bot_mode = MODE_BACK_OUT; timer = 80; //This determines how far we back up } } if(timer == 0){ if(line_correlation > 23){ bot_mode = MODE_BACKUP_TO_LINE; timer = 30; // 0.3 sec limit_timer = 15; // 1.5 second } } break; case MODE_BACKUP_TO_LINE: i = clip_neg(line_find); r_speed = addsat(REV2, 0-i); //Backup to line l_speed = addsat(REV2, 0-i); if(timer == 0){ if(motors_stopped()){ bot_mode = MODE_CENTER; //found it, now center on it timer = 50; limit_timer = 20; //2 sec to center on it } if(limit_timer == 0){ timer = 50; bot_mode = MODE_FINDLINE; } } break; case MODE_CENTER: r_speed = addsat(0, 0-line_find); //Center rear of robot on line l_speed = addsat(0, 0-line_find); if(timer == 0){ if(motors_stopped()){ bot_mode = MODE_ROTATE_R; timer = 50; limit_timer = 35; //3.5 seconds allowed to aquire line } // This is 360 degrees of rotation if(limit_timer == 0){ bot_mode = MODE_FINDLINE; timer = 50; } } break; case MODE_FREE_ROTATE_R: r_speed = REV3; l_speed = FWD3; if(timer == 0){ bot_mode = MODE_ROTATE_R; timer = 50; limit_timer = 35; //3.5 seconds allowed to aquire line } break; case MODE_BACK_OUT_LEFT: set_bit(mode_bits,DIR); bot_mode = MODE_BACK_OUT; break; case MODE_BACK_OUT_RIGHT: clear_bit(mode_bits,DIR); bot_mode = MODE_BACK_OUT; break; case MODE_BACK_OUT: r_speed = REV2; l_speed = REV2; if(timer == 0){ bot_mode = MODE_ROTATE_BACK_OUT; timer = 50; //0.5 sec for about 120 deg rotation } break; case MODE_ROTATE_BACK_OUT: if(mode_bits & (1 << DIR)){ r_speed = FWD3; //rotate left l_speed = REV3; }else{ r_speed = REV3; //rotate right l_speed = FWD3; } if(timer == 0){ bot_mode = MODE_FINDLINE; timer = 50; } break; case MODE_BACKUP_AND_TURN_AROUND: r_speed = REV3; l_speed = REV3; if(timer == 0){ bot_mode = MODE_FREE_ROTATE_R; timer = 50; } break; }//End switch for (i=0;i<3;i++) { //Adjust motor speeds based on feedback values rmfb = ad_convert(AD_RMFB); //get right motor feedback lmfb = ad_convert(AD_LMFB); //get left motor feedback r_integrate = integrate(r_speed,rmfb,r_integrate); l_integrate = integrate(l_speed,lmfb,l_integrate); right_speed = r_integrate; left_speed = l_integrate; } if (txstate == 0) { make_info_pkt(); //Get new data for transmission } send_tx_buffer(); //Send data on RF link if TX ready if(c_timer == 0){ //update line detection correlator every 40ms line_correlation = detect_line(); if(line_correlation > peak_correlation) peak_correlation = line_correlation; c_timer = 4; } } } /* End of source */