;******************************************************************* ;Dbt_light_seek ;Derbot seeks light. PWM applied. Speed is dependent on ;light difference (front to back), so Derbot comes to a ;halt when light difference is minimal. Microswitches used ;for bump detection. ; ;TJW 19.5.05 Tested 19.5.05 ;******************************************************************* ;Clock is 4MHz ; list p=16F873A #include p16f873A.inc ; ;Set Configuration Word: crystal oscillator HS, WDT off, ; power-up timer on, code protect off, LV Program off. __CONFIG _HS_OSC & _WDT_OFF & _PWRTE_ON & _LVP_OFF ; ;Specify RAM delcntr1 equ 20 ;used in delay5 & delayADC SRs delcntr2 equ 21 ldr_rt equ 22 ;stores for ldr values, read thru ADC ldr_left equ 23 ldr_rear equ 24 ldr_ave equ 25 ;computed average of front ldrs ldr_diff equ 26 ;difference between front ldrs, left-right ldr_fwd equ 27 ;ave fwd speed, if used temp equ 28 ;a temp location, to be used only in consecutive instructions ;Specify some port bits ;For Port A mot_en_rt equ 2 mot_en_left equ 5 ;For Port B sounder equ 1 ;piezo electric sounder us_rt equ 4 ;right microswitch us_left equ 5 ;left microswitch ;For PortC mot_left equ 1 ;left motor direction bit, 1=forward mot_rt equ 2 ;right motor direction bit led_rt equ 5 ;diagnostic led led_left equ 6 ;diagnostic led mode equ 7 ;mode switch ; org 00 ;Initialise bcf status,rp1 bsf status,rp0 ;select memory bank 1 movlw B'00001011' ;set port A bits according to their function movwf trisa movlw B'11001000' movwf trisb ;also port B bits movlw B'10000000' movwf trisc ;and port C bits movlw B'00000100' ;Set up ADC, left justified result, movwf adcon1 ;port A bits 0,1,3 for analog input movlw 0ff ;set PWM prd movwf pr2 bcf status,rp0 ;select bank 0 ;set up PWM movlw B'00000100' ;switch on Timer2, no pre or postscale movwf t2con movlw B'00001100' ;enable PWM movwf ccp1con movwf ccp2con movlw B'01000001' ;Set up ADC, clock Fosc/8, switch ADC on but not running, movwf adcon0 ;input channel selection currently irrelevant ; ;Switch all outputs off movlw 00 movwf porta movwf portb movwf portc ;diagnostic, flash leds bsf portc,6 bsf portc,5 call delay500 bcf portc,6 bcf portc,5 call delay500 ;Read mode switch to determine Derbot operating mode btfsc portc,mode goto sw_move bsf porta,mot_en_left ;enable motors permanently bsf porta,mot_en_rt ;*********************************** ;The “main” program loop starts here ;*********************************** ;check first for collisions light_loop btfss portb,us_rt ;check right microswitch call rev_rt btfss portb,us_left ;check left microswitch call rev_left ; ;read and store ldrs movlw B'01000001' ;select channel 0 (left ldr) movwf adcon0 call delay11u ;acquisition time bsf adcon0,go ;start conversion btfsc adcon0,go ;wait for conversion to complete goto $-1 movf adresh,0 sublw 0ff ;reverse "polarity" of result movwf ldr_left movlw B'01001001' ;select channel 1 (right ldr) movwf adcon0 call delay11u bsf adcon0,go ;start conversion btfsc adcon0,go ;wait for conversion to complete goto $-1 movf adresh,0 sublw 0ff ;reverse "polarity" of result movwf ldr_rt movlw B'01011001' ;select channel 3 (rear ldr) movwf adcon0 call delay11u bsf adcon0,go ;start conversion btfsc adcon0,go ;wait for conversion to complete goto $-1 movf adresh,0 sublw 0ff ;reverse "polarity" of result movwf ldr_rear ;divide all values by 2, to avoid over-range when adding PWM offset bcf status,c rrf ldr_rt,1 bcf status,c rrf ldr_left,1 bcf status,c rrf ldr_rear,1 ;average front two and store movf ldr_rt,0 ;add two front ldr values addwf ldr_left,0 movwf ldr_ave rrf ldr_ave,1 ;divide by two, this is the average ;determine ave fwd velocity term, which may be used later movf ldr_rear,0 ;subtract rear from ave subwf ldr_ave,0 movwf ldr_fwd ;test for brightest, and take action movf ldr_rt,0 ;subtract right from left subwf ldr_left,0 movwf ldr_diff ;save the difference value btfss status,c ;skip if ldr_left>ldr_rt goto rt_gt_left ;here if ldr_left>ldr_rt movf ldr_rear,0 ;subtract rear from left subwf ldr_left,0 btfss status,c ;skip if ldr_left>ldr_rear goto rot_left ;go here if rear is brightest goto lgt_fwd_l ;light is front left, hence move fwd left ;here if ldr_rt>ldr_left rt_gt_left comf ldr_diff,0 ;this will be holding a negative no, addlw 1 ;therefore correct by taking 2's comp. movwf ldr_diff movf ldr_rear,0 ;subtract rear from right subwf ldr_rt,0 btfss status,c ;skip if ldr_rt>ldr_rear goto rot_rt goto lgt_fwd_r ;light is front rt, hence move fwd rt ;fixed speed rotation, if light is at rear rot_left call rtmot_fwd call leftmot_rev goto loopend rot_rt call leftmot_fwd call rtmot_rev goto loopend ;light is front left, hence move forward left lgt_fwd_l movf ldr_diff,0 addwf ldr_fwd,0 movwf temp btfsc status,c ;check for overflows movlw 7f ;will use this value if overflowed btfsc temp,7 movlw 7f movwf temp bcf status,c btfss temp,6 rlf temp,1 ;scale up this value btfss temp,6 rlf temp,1 ;scale up this value btfss temp,6 rlf temp,1 ;scale up this value movf temp,0 addlw 80 ;offset for PWM movwf CCPR1L ;set right motor, which is greater movf ldr_diff,0 subwf ldr_fwd,0 movwf temp btfss status,c ;has subtraction caused borrow? clrf temp ;if yes, clear temp to 0 bcf status,c btfss temp,6 rlf temp,1 ;scale up this value btfss temp,6 rlf temp,1 ;scale up this value btfss temp,6 rlf temp,1 ;scale up this value movf temp,0 addlw 80 ;offset for PWM movwf CCPR2L ;drive left motor weaker goto loopend ; ;light is front rt, hence move forward rt lgt_fwd_r movf ldr_diff,0 addwf ldr_fwd,0 movwf temp btfsc status,c ;check for overflows movlw 7f btfsc temp,7 movlw 7f movwf temp bcf status,c btfss temp,6 rlf temp,1 ;scale up this value btfss temp,6 rlf temp,1 ;scale up this value btfss temp,6 rlf temp,1 ;scale up this value movf temp,0 addlw 80 ;offset for PWM movwf CCPR2L ;drive left motor harder movf ldr_diff,0 subwf ldr_fwd,0 movwf temp btfss status,c ;has subtraction caused borrow? clrf temp ;if yes, clear temp to 0 bcf status,c btfss temp,6 rlf temp,1 ;scale up this value btfss temp,6 rlf temp,1 ;scale up this value btfss temp,6 rlf temp,1 ;scale up this value movf temp,0 addlw 80 ;offset for PWM movwf CCPR1L ;drive right motor weaker goto loopend ; loopend call delay200 movlw B'01000000' ;toggle led xorwf portc,1 goto light_loop ; ;************************************* ;move microswitch states to diag leds ;************************************* sw_move bcf portc,led_left ;preclear left led btfss portb,us_left bsf portc,led_left ;but set it if button pressed ; bcf portc,led_rt ;preclear port C, bit 5 btfss portb,us_rt bsf portc,led_rt ;but set it if button pressed goto sw_move ;********************************************** ;SUBROUTINES ;********************************************** ;Right microswitch "bump" detected. AGV reverses, and turns left. rev_rt bsf portc,led_rt bcf porta,mot_en_left ;stop motors bcf porta,mot_en_rt bsf portb,sounder call delay200 bcf portb,sounder ;both motors in reverse call leftmot_rev call rtmot_rev call delay500 call delay500 call leftmot_fwd ;left motor forward to turn call delay500 call delay500 bcf portc,led_rt return ; ;Left microswitch "bump" detected. AGV reverses, and turns right. rev_left bsf portc,led_left bcf porta,mot_en_rt ;stop motors bcf porta,mot_en_left bsf portb,sounder call delay200 bcf portb,sounder ;both motors in reverse call leftmot_rev call rtmot_rev call delay500 call delay500 call rtmot_fwd ;right motor forward to turn call delay500 call delay500 bcf portc,led_left return ;introduces delay of 11us approx delay11u movlw D'2' ;5 cycles called, ;each taking 3us, plus call, return (2 ea), and 2 move insts ;less one cycle lost when last goto is hopped movwf delcntr1 dela decfsz delcntr1,1 ;3 inst cycles in this loop, ie 3us goto dela return ; ;introduces delay of 1ms approx delay1 movlw D'250' ;250 cycles called, ; each taking 4us movwf delcntr1 del1 nop ;4 inst cycles in this loop, ie 4us decfsz delcntr1,1 goto del1 return ; ;200ms delay (approx) ;200 calls to delay1 delay200 movlw D'200' movwf delcntr2 del2 call delay1 decfsz delcntr2,1 goto del2 return ; ;500ms delay (approx) ;500 calls to delay1 delay500 movlw D'250' movwf delcntr2 del5 call delay1 call delay1 decfsz delcntr2,1 goto del5 return ;motor drive SRs, set motors running fixed (modest) speed leftmot_fwd ;sets left motor running forward bsf porta,mot_en_left movlw D'196' movwf CCPR2L return rtmot_fwd bsf porta,mot_en_rt movlw D'196' movwf CCPR1L return leftmot_rev bsf porta,mot_en_left movlw D'60' movwf CCPR2L return rtmot_rev bsf porta,mot_en_rt movlw D'60' movwf CCPR1L return end