'**************************************************************** '* Name : TEMPLATE20.BAS * '* Author : Giuseppe Marullo * '* Notice : Copyright (c) 2002 Copyright 2002 * '* : All Rights Reserved * '* Date : 12/12/2002 * '* Version : 1.0 * '* Notes : * '* : Modified to get neutral at 127 * '* : * '* : * '* : TO DO: * '* : - Complete the defines for Controller board * '* : - Complete the defines for Sensor Board * '* : - optimize the IRQ routine * '* : - Routines for Sensor Board * '* : - Init * '* : - Beep * '* : * '**************************************************************** ' '**************************************************************** '* * '* Part 1 - Defines and variables * '* * '**************************************************************** ' ' ' ' Oscillator Speed - 20MHz BEWARE of 4MHz pics DEFINE OSC 20 ' HW Serial Port Speed DEFINE HSER_BAUD 38400 ' Use 100KHz (initial I2C Specs) speed DEFINE I2C_SLOW 1 ' ' Interrupt Variables - Useful to save bank infos wsave var byte $20 system ssave var byte bank0 system psave var byte bank0 system 'wsave1 var BYTE $0a0 SYSTEM ; not needed with 877? 'wsave2 var BYTE $120 SYSTEM ; not needed with 877? 'wsave3 var BYTE $1a0 SYSTEM ; not needed with 877? ' Keeps the servo's position. One for left and the other for right MAX_SPEED con 30 MAX_ACC con 1 NEUTRAL_SERVO con 127 SERVOL var BYTE SERVOR var BYTE ' Stores the Right Sharp Reading REYE var word REYE_H var REYE.BYTE1 REYE_L var REYE.BYTE0 ' Stores the Left Sharp Reading LEYE var word LEYE_H var LEYE.BYTE1 LEYE_L var LEYE.BYTE0 WVAR_H var BYTE WVAR_L var BYTE ' Stores the Right Line Sensor's reading LINER var word LINER_H var LINER.byte1 LINER_L var LINER.byte0 ' Stores the Left Line Sensor's reading LINEL var word LINEL_H var LINEL.byte1 LINEL_L var LINEL.byte0 ' Stores the Center Line Sensor's reading LINEC var word LINEC_H var LINEC.byte1 LINEC_L var LINEC.byte0 TEMP_STRING var byte[5] ' Where the buzzer is connected BUZZER_PIN var PORTC.5 ' Where the accelerometer is connected X Axis XAXIS_PIN var PORTB.6 ' Where the accelerometer is connected Y Axis YAXIS_PIN var PORTB.7 ' Accelerometer's variables T2 var word T2_H var T2.Byte1 T2_L var T2.Byte0 CNT var byte TEMP var word TEMP_H var TEMP.Byte1 TEMP_L var TEMP.Byte0 TEMP2 var word TEMP2_H var TEMP2.Byte1 TEMP2_L var TEMP2.Byte0 XAXIS var word XAXIS_H var XAXIS.Byte1 XAXIS_L var XAXIS.Byte0 MIN_XAXIS var word MAX_XAXIS var word MIN_YAXIS var word MAX_YAXIS var word YAXIS var word YAXIS_H var YAXIS.Byte1 YAXIS_L var YAXIS.Byte0 XAXIS0 var word XAXIS0_H var XAXIS0.Byte1 XAXIS0_L var XAXIS0.Byte0 YAXIS0 var word YAXIS0_H var YAXIS0.Byte1 YAXIS0_L var YAXIS0.Byte0 ' I2C connection SDA_PIN var PORTC.4 SCL_PIN var PORTC.3 ' I2C Variables device var byte ' device 0-7 directions var byte cont var byte o_patt var byte ' byte to appear on output of 8574 i_patt var byte ' byte read from input of 8574 adc_value var byte[2] '**************************************************************** '* * '* Part 2 - GOTO START * '* * '**************************************************************** Goto start ' Skip around interrupt handler ' ' '**************************************************************** '* * '* Part 3 - INTERRUPT ROUTINE * '* * '**************************************************************** ' DEFINE INTHAND myint asm AUX1_L EQU 0X24 SERVOVALUE EQU 0X45 I EQU 0X42 ; Save W, STATUS and PCLATH registers myint ; Insert interrupt qcode here bcf STATUS, RP1 bcf STATUS, RP0 bsf PORTB , 1 MOVF _SERVOR, W MOVWF SERVOVALUE PAGESEL (DODUTYDELAY) CALL DODUTYDELAY BCF STATUS, RP1 BCF STATUS, RP0 MOVF SERVOVALUE, W MOVWF _SERVOR BCF PORTB, 1 BCF PORTB, 1 bsf PORTB , 2 MOVF _SERVOL, W MOVWF SERVOVALUE PAGESEL (DODUTYDELAY) CALL DODUTYDELAY BCF STATUS, RP1 BCF STATUS, RP0 MOVF SERVOVALUE, W MOVWF _SERVOL BCF PORTB, 2 BCF PORTB, 2 ; Save and restore FSR if used ; Restore PCLATH, STATUS and W registers bcf PIR1 , 0 ; Clear Timer 1 Interrupt Flag movf psave, W movwf PCLATH swapf ssave, W movwf STATUS swapf wsave, F swapf wsave, W retfie DODUTYDELAY BCF STATUS, RP1 BCF STATUS, RP0 MOVLW 1 MOVWF I AUXLAB2 BCF STATUS, RP1 BCF STATUS, RP0 PAGESEL(DELAYLAB13);* ; MOVLW 165;* MOVLW 14 ; to get neutral position at 128 - TO BE TESTED MOVWF AUX1_L DELAYLAB13;* DECFSZ AUX1_L, F;* GOTO DELAYLAB13;* NOP;* NOP;* PAGESEL (NEXTLAB2) MOVF SERVOVALUE, W SUBWF I, W BTFSC STATUS, Z GOTO NEXTLAB2 INCF I, F PAGESEL (AUXLAB2) GOTO AUXLAB2 NEXTLAB2 BCF STATUS, RP1 BCF STATUS, RP0 RETURN READA2D BCF STATUS, RP1 BCF STATUS, RP0 ; READ THE A2D AND STORE THE ANSWER IN WVAR ; DELAY 0.02, 0 ; NEED TO WAIT AT LEAST 2TAD (32 20MHZ CLOCK CYCLES 3.2USEC) ; BETWEEN A/D SAMPLINGS. PAGESEL(DELAYLAB17);* MOVLW 32;* MOVWF AUX1_L DELAYLAB17;* DECFSZ AUX1_L, F;* GOTO DELAYLAB17;* NOP;* BSF ADCON0, GO PAGESEL(DELAYLAB18);* MOVLW 65;* MOVWF AUX1_L DELAYLAB18;* DECFSZ AUX1_L, F;* GOTO DELAYLAB18;* NOP;* NOP;* ADCLAB9 BCF STATUS, RP1 BCF STATUS, RP0 PAGESEL (ADCLAB9) BTFSC ADCON0, GO GOTO ADCLAB9 BSF STATUS, RP0 MOVF ADRESL, W BCF STATUS, RP0 MOVWF _WVAR_L MOVF ADRESH, W MOVWF _WVAR_H BCF ADCON0, ADON RETURN _READEYES BCF STATUS, RP1 BCF STATUS, RP0 BSF STATUS, RP0 BCF PIE1, ADIE MOVLW 145 BCF STATUS, RP0 MOVWF ADCON0 MOVLW 0 BSF STATUS, RP0 MOVWF ADCON1 BCF STATUS, RP0 BSF ADCON0, ADON PAGESEL (READA2D) CALL READA2D BCF STATUS, RP1 BCF STATUS, RP0 MOVF _WVAR_H, W MOVWF _REYE_L CLRF _REYE_H BSF STATUS, RP0 BCF PIE1, ADIE MOVLW 153 BCF STATUS, RP0 MOVWF ADCON0 MOVLW 0 BSF STATUS, RP0 MOVWF ADCON1 BCF STATUS, RP0 BSF ADCON0, ADON PAGESEL (READA2D) CALL READA2D BCF STATUS, RP1 BCF STATUS, RP0 MOVF _WVAR_H, W MOVWF _LEYE_L CLRF _LEYE_H RETURN _READSENSORS BCF STATUS, RP1 BCF STATUS, RP0 BSF STATUS, RP0 BCF PIE1, ADIE MOVLW 169 BCF STATUS, RP0 MOVWF ADCON0 MOVLW 0 BSF STATUS, RP0 MOVWF ADCON1 BCF STATUS, RP0 BSF ADCON0, ADON PAGESEL (READA2D) CALL READA2D BCF STATUS, RP1 BCF STATUS, RP0 MOVF _WVAR_H, W MOVWF _LINER_L CLRF _LINER_H BSF STATUS, RP0 BCF PIE1, ADIE MOVLW 177 BCF STATUS, RP0 MOVWF ADCON0 MOVLW 0 BSF STATUS, RP0 MOVWF ADCON1 BCF STATUS, RP0 BSF ADCON0, ADON PAGESEL (READA2D) CALL READA2D BCF STATUS, RP1 BCF STATUS, RP0 MOVF _WVAR_H, W MOVWF _LINEC_L CLRF _LINEC_H BSF STATUS, RP0 BCF PIE1, ADIE MOVLW 185 BCF STATUS, RP0 MOVWF ADCON0 MOVLW 0 BSF STATUS, RP0 MOVWF ADCON1 BCF STATUS, RP0 BSF ADCON0, ADON PAGESEL (READA2D) CALL READA2D BCF STATUS, RP1 BCF STATUS, RP0 MOVF _WVAR_H, W MOVWF _LINEL_L CLRF _LINEL_H BCF STATUS, RP1 BCF STATUS, RP0 RETURN endasm '**************************************************************** '* * '* Part 4 - START LABEL - USER PROGRAM STARTS HERE * '* * '**************************************************************** start: ' ' Initialize timers and other stuff useful with Interrupt ' asm MOVLW B'00010000' BCF STATUS, RP0 MOVWF T1CON BCF STATUS, RP0 BCF PIR1, TMR1IF MOVLW 60 MOVWF TMR1H MOVLW 176 MOVWF TMR1L BSF T1CON, TMR1ON BSF STATUS, RP0 BSF PIE1, TMR1IE BCF STATUS, RP0 endasm ' Be sure that interrupts are disabled asm BCF INTCON, GIE BCF INTCON, PEIE endasm HIGH PORTB.1 HIGH PORTB.2 HIGH PORTB.3 HIGH PORTB.4 SERVOL = NEUTRAL_SERVO ' Neutral position - Activate Interrupts to make the robot move! SERVOR = NEUTRAL_SERVO ' Neutral position - Activate Interrupts to make the robot move! directions=$00 device = $7 loop: HSERIN [STR TEMP_STRING\2\13] IF TEMP_STRING[0] = "l" THEN HSEROUT ["Left Servo Test", 13, 10] SERVOL = NEUTRAL_SERVO + 10 PAUSE 1000 SERVOL = NEUTRAL_SERVO ENDIF IF TEMP_STRING[0] = "r" THEN HSEROUT ["Right Servo Test", 13, 10] SERVOR = NEUTRAL_SERVO - 10 PAUSE 1000 SERVOR = NEUTRAL_SERVO ENDIF if TEMP_STRING[0] = "e" then HSEROUT ["I2C ADC Test", 13, 10] ' ' Dall'esterno ' marrone ' blu ' giallo ' verde ' Control Byte for ADC ' 1000 1100 ' 1------------- START - ' 0------------ SEL2 ADC Channel Selector - ' 0----------- SEL1 ADC Channel Selector - ' 0---------- SEL0 ADC Channel Selector - ' 1-------- RNG - +- 10V ' 1------- BIP - +- 10V ' 0------ PD1 - No Power Down ' 0----- PD0 - No Power Down ' 10001100 = device = %1010001 ' ADC address MAX127 101 with Address Pins all 0 I2CWRITE SDA_PIN,SCL_PIN, device,%10001100,[0],i2c_timeout2 I2CREAD SDA_PIN, SCL_PIN, device, %10001100,[STR adc_value\2], i2c_timeout2 HSEROUT ["ADC Channel0 ",dec adc_value[1]," ",dec adc_value[2],13,10] I2CREAD SDA_PIN, SCL_PIN, device, %10011100,[STR adc_value\2], i2c_timeout2 HSEROUT ["ADC Channel1 ",dec adc_value[1]," ",dec adc_value[2],13,10] I2CREAD SDA_PIN, SCL_PIN, device, %10101100,[STR adc_value\2], i2c_timeout2 HSEROUT ["ADC Channel2 ",dec adc_value[1]," ",dec adc_value[2],13,10] I2CREAD SDA_PIN, SCL_PIN, device, %10111100,[STR adc_value\2], i2c_timeout2 HSEROUT ["ADC Channel3 ",dec adc_value[1]," ",dec adc_value[2],13,10] I2CREAD SDA_PIN, SCL_PIN, device, %11001100,[STR adc_value\2], i2c_timeout2 HSEROUT ["ADC Channel4 ",dec adc_value[1]," ",dec adc_value[2],13,10] I2CREAD SDA_PIN, SCL_PIN, device, %11011100,[STR adc_value\2], i2c_timeout2 HSEROUT ["ADC Channel5 ",dec adc_value[1]," ",dec adc_value[2],13,10] I2CREAD SDA_PIN, SCL_PIN, device, %11101100,[STR adc_value\2], i2c_timeout2 HSEROUT ["ADC Channel6 ",dec adc_value[1]," ",dec adc_value[2],13,10] I2CREAD SDA_PIN, SCL_PIN, device, %11111100,[STR adc_value\2], i2c_timeout2 HSEROUT ["ADC Channel7 ",dec adc_value[1]," ",dec adc_value[2],13,10] goto skip2 i2c_timeout2: HSEROUT ["ADC timeout", bin device ,13,10] skip2: endif if TEMP_STRING[0] = "j" then HSEROUT ["I2C Bus Scan...",13,10] for device = 0 to 127 i_patt = device <<1 i_patt = i_patt | %00000001 I2CREAD SDA_PIN, SCL_PIN,i_patt,[i_patt], i2c_timeout ' HSEROUT [" Device ",hex device, " is present on I2C bus",13,10] if (device >= 80) and (device < 88) then HSEROUT [" I2C 24FC256 is present. Address is ",dec device ,13,10] endif if (device >= 32) and (device < 40) then HSEROUT [" I2C PCF8574 is present. Address is ",dec device ,13,10] i_patt = device <<1 i_patt = i_patt o_patt = 0 I2CWRITE SDA_PIN, SCL_PIN,i_patt,0, i2c_timeout PAUSE 1000 i_patt = i_patt o_patt = 255 I2CWRITE SDA_PIN, SCL_PIN,i_patt,255, i2c_timeout PAUSE 1000 endif if (device >= 40) and (device < 48) THEN HSEROUT [" I2C MAX127 is present. Address is ",dec device ,13,10] i_patt = device <<1 i_patt = i_patt | %00000001 I2CWRITE SDA_PIN, SCL_PIN,i_patt,[i_patt], i2c_timeout endif goto skip_timeout i2c_timeout: ' HSEROUT [" Device ",bin device, " is NOT present on I2C bus",13,10] ' HSEROUT [" Device ",bin 1 | (device<<1), " is NOT present on I2C bus",13,10,13,10] skip_timeout: next endif 'cippa if temp_string[0] = "p" then I2CWRITE SDA_PIN, SCL_PIN,64,0, i2c_timeout3 PAUSE 1000 I2CWRITE SDA_PIN, SCL_PIN,64,255, i2c_timeout3a PAUSE 1000 goto skip_tt i2c_timeout3: HSEROUT ["Timeout! on First Write",13,10] i2c_timeout3a: HSEROUT ["Timeout! on Second Write",13,10] HSEROUT ["Searching for PCF8574:",13,10] skip_tt: for device = 1 to 255 I2CREAD SDA_PIN,SCL_PIN,device,i_patt,[i_patt],i2c_timeout4 HSEROUT ["Device found at ",dec device,13,10] i2c_timeout4: next device endif IF TEMP_STRING[0] = "s" THEN HSEROUT ["Enable Interrupt",13,10] asm BSF INTCON, GIE BSF INTCON, PEIE endasm ENDIF IF TEMP_STRING[0] = "t" THEN HSEROUT ["Disable Interrupt",13,10] asm BCF INTCON, GIE BCF INTCON, PEIE endasm ENDIF IF TEMP_STRING[0] = "b" THEN HSEROUT ["Beep Test",13,10] FREQOUT BUZZER_PIN, 500, 1000 ENDIF IF TEMP_STRING[0] = "k" THEN CALL READEYES CALL READSENSORS HSEROUT [68,17,69,21,"@",DEC LEYE,"*",DEC REYE,"*",DEC LINEL,"*",DEC LINEC,"*",DEC LINER,"?",13,10] endif IF TEMP_STRING[0] = "w" THEN T2 = 0 XAXIS = 0 YAXIS = 0 TEMP = 0 TEMP2 = 0 COUNT XAXIS_PIN, 1000, T2 MIN_XAXIS = 32000 MAX_XAXIS = 0 FOR CNT = 1 to 10 PULSIN XAXIS_PIN,1,XAXIS 'read T1x TEMP = TEMP + XAXIS IF XAXIS > MAX_XAXIS then MAX_XAXIS = XAXIS endif IF XAXIS < MIN_XAXIS then MIN_XAXIS = XAXIS endif NEXT CNT MIN_YAXIS = 32000 MAX_YAXIS = 0 FOR CNT = 1 to 10 PULSIN YAXIS_PIN,1,YAXIS 'read T1x TEMP2 = TEMP2 + YAXIS IF YAXIS > MAX_YAXIS then MAX_YAXIS = YAXIS endif IF YAXIS < MIN_YAXIS then MIN_YAXIS = YAXIS endif NEXT CNT HSEROUT [68,17,69,21,"@",DEC T2,"*",DEC TEMP,"*",DEC TEMP2,"?",13,10] FREQOUT BUZZER_PIN, 10, 2000 Loop003: if XAXIS_PIN = 1 then PAUSEUS 2 GOTO loop003 endif ENDIF IF TEMP_STRING[0] = "a" THEN PAUSE 10 PULSIN XAXIS_PIN,1, XAXIS PAUSE 10 PULSIN YAXIS_PIN,1, YAXIS PAUSE 10 PULSIN XAXIS_PIN,0, XAXIS0 PAUSE 10 PULSIN XAXIS_PIN,0, YAXIS0 PAUSE 10 HSEROUT ["@","@@", XAXIS_H,XAXIS_L,YAXIS_H,YAXIS_L, XAXIS0_H, XAXIS0_L, YAXIS0_H, YAXIS0_L,"*",13,10] ENDIF IF RCSTA.1 = 1 THEN HSEROUT ["Rx Overrun - Fixing",13,10] ASM BCF RCSTA, CREN BSF RCSTA, CREN ENDASM HSEROUT ["Rx Overrun - Fixed",13,10] ENDIF if TEMP_STRING[0] = "h" then DEFINE HPWM2_TIMER 1 HSEROUT ["HW PWM 2",13,10] PAUSE 5000 HPWM 2, 127,10000 HPWM 1, 0,0 HPWM 2, 0,0 endif if TEMP_STRING[0] = "m" then asm BSF INTCON, GIE BSF INTCON, PEIE endasm SERVOL = NEUTRAL_SERVO SERVOR = NEUTRAL_SERVO LLLL: I2CWRITE SDA_PIN, SCL_PIN,64,0 CALL READEYES CALL READSENSORS HSEROUT ["L=",dec LEYE,"R=",dec REYE," ", dec SERVOL," ", dec SERVOR, "SL=",dec LINEL,"RL",dec LINER,13,10] IF LINEC > 200 then SERVOL = NEUTRAL_SERVO - 10 SERVOR = NEUTRAL_SERVO + 10 PAUSE 400 SERVOL = NEUTRAL_SERVO + 10 SERVOR = NEUTRAL_SERVO - 10 PAUSE 400 endif IF LINEL > 200 then SERVOL = SERVOL + 5 SERVOR = NEUTRAL_SERVO PAUSE 400 endif IF LINER > 200 then SERVOL = NEUTRAL_SERVO SERVOR = SERVOR - 5 PAUSE 400 endif IF LEYE > 20 THEN IF REYE > 20 THEN asm BCF INTCON, GIE BCF INTCON, PEIE endasm I2CWRITE SDA_PIN, SCL_PIN,64,6 asm BSF INTCON, GIE BSF INTCON, PEIE endasm IF LEYE >= REYE THEN SERVOL = SERVOL + 1 ENDIF PAUSE 50 IF SERVOL > (NEUTRAL_SERVO + MAX_SPEED) THEN SERVOL = NEUTRAL_SERVO + MAX_SPEED ENDIF IF LEYE <= REYE THEN SERVOR = SERVOR - 1 ENDIF PAUSE 50 IF SERVOR < (NEUTRAL_SERVO - MAX_SPEED) THEN SERVOR = NEUTRAL_SERVO ENDIF ELSE asm BCF INTCON, GIE BCF INTCON, PEIE endasm I2CWRITE SDA_PIN, SCL_PIN,64,4 asm BSF INTCON, GIE BSF INTCON, PEIE endasm SERVOL = SERVOL - 1 PAUSE 50 IF SERVOL < NEUTRAL_SERVO THEN SERVOL = NEUTRAL_SERVO ENDIF SERVOR = SERVOR - 1 PAUSE 50 IF SERVOR < (NEUTRAL_SERVO - MAX_SPEED) THEN SERVOR = NEUTRAL_SERVO - MAX_SPEED ENDIF endif else IF REYE > 30 THEN asm BCF INTCON, GIE BCF INTCON, PEIE endasm I2CWRITE SDA_PIN, SCL_PIN,64,2 asm BSF INTCON, GIE BSF INTCON, PEIE endasm SERVOL = SERVOL + 1 PAUSE 50 IF SERVOL > (NEUTRAL_SERVO + MAX_SPEED) THEN SERVOL = NEUTRAL_SERVO + MAX_SPEED ENDIF SERVOR = SERVOR + 1 PAUSE 50 IF SERVOR > NEUTRAL_SERVO THEN SERVOR = NEUTRAL_SERVO ENDIF ELSE SERVOL = SERVOL - 1 PAUSE 50 IF SERVOL < NEUTRAL_SERVO THEN SERVOL = NEUTRAL_SERVO ENDIF SERVOR = SERVOR + 1 PAUSE 50 IF SERVOR > NEUTRAL_SERVO THEN SERVOR = NEUTRAL_SERVO ENDIF asm BCF INTCON, GIE BCF INTCON, PEIE endasm I2CWRITE SDA_PIN, SCL_PIN,64,0 asm BSF INTCON, GIE BSF INTCON, PEIE endasm ; No target in sight - Spin for a random time seeking opponent endif ENDIF GOTO LLLL endif goto loop