; ernesto 1.0, May 1st 2001, by David Sharp ; cockroach behavior using two touch sensors, an ambient light sensor, ; and two directional light sensors. behavior: run for the shadows! assumes ; two servos modified for continual motion (but neither modified to spin in ; reverse). A PIC16F84 with a 3.58MHz clock is assumed (sorry! hehe). ; concept: multiple "instinct" subroutines called in order. all use the same ; operator subroutines (tho weighted differently: touch -> ambient light ; -> directional light). this version uses no analog/digital converters so ; each "instinct" is either fully on or fully off. the operators change speed ; variables for left and right wheels, and once all instincts have been called ; the servo routine sends the corresponding signal to move the roach. ; instinct if activated call operator ;--------------------------------------------------- ; explore (always on) forward ; ; touch left rotate_right, reverse ; right rotate_left, reverse ; ; ambient on forward ; ; directional left rotate_right ; right rotate_left ; ; wander (always on) (noisify speeds) ; ; notes: loop always starts out set to "wander" forward. ; update_servos adds some random noise to counteract oscillating etc. ; store velocity values in bytes. "wander" sets them to 127, then ; other instincts alter them. update_servos randomizes and clamps them. ; 3.58mhz clock, / 4 = 895000, / 64 (prescaler) = 13984 Hz. ; 1 second = 92x152 ticks of the clock ; for servos, range is 14-21-28 (reverse-still-forward) #include "P16F84.INC" ; Include header file LIST P=16F84, R=DEC ; Use the PIC16F84 and decimal system __config _XT_OSC & _WDT_OFF & _PWRTE_ON ; declarations porta equ 05h sensors equ porta portb equ 06h outputs equ portb trisa equ 85h trisb equ 86h status equ 03h ; variable declarations counter equ 0x00C ; general purpose counter slowTimer equ 0x00D ; acts as a slower version of TMR0 leftServoV equ 0x00E ; velocity for left servo rightServoV equ 0x00F ; velocity for right servo leftTouchTimer equ 0x010 ; counts down from last collision on left side rightTouchTimer equ 0x011 ; counts down from last collision on right side ; servo constants leftServo equ 0 ; pin on portb for left servo rightServo equ 1 ; pin on portb for right servo servoSpacer equ 255 ; space between servo pulses walkingSpeed equ 1 restingSpeed equ 21 collisionDelay equ 255 ; time to reset touchTimers to on collision ; sensors (on porta) touchLeft equ 0 touchRight equ 1 ambientLight equ 2 lightLeft equ 3 lightRight equ 4 init movlw b'00000101' ; set clock prescaler to 64 option ; clrf sensors clrf outputs clrf TMR0 bsf status,5 movlw b'00011111' ; setup porta as all inputs movwf trisa ; movlw b'00000000' ; setup portb as all inputs movwf trisb ; bcf status,5 ; cockroach behaviors! start call rest call move_forward call avoid_light_sources call avoid_ambient_light call avoid_touch ; last so it can override everything call update_roach_speed ; changes take effect, roach moves goto start ; instincts avoid_touch btfsc sensors,touchLeft goto avoid_left_touch check_right_touch btfss sensors,touchRight return avoid_right_touch call rest call rotate_left call move_reverse call move_reverse return avoid_left_touch call rest call rotate_right call move_reverse call move_reverse goto check_right_touch avoid_ambient_light btfss sensors,ambientLight return call full_speed_ahead return avoid_light_sources btfsc sensors,lightLeft goto avoid_left_light check_right_light btfss sensors,lightRight return avoid_right_light call rotate_left call rotate_left return avoid_left_light call rotate_right call rotate_right goto check_right_light ; movement operators rest movlw restingSpeed movwf leftServoV movwf rightServoV return rest_left movlw restingSpeed movwf leftServoV return rest_right movlw restingSpeed movwf rightServoV return move_forward movlw walkingSpeed addwf rightServoV subwf leftServoV ; (left servo runs backwards) return full_speed_ahead call move_forward call move_forward call move_forward call move_forward call move_forward call move_forward call move_forward return move_reverse movlw walkingSpeed subwf rightServoV addwf leftServoV return rotate_left movlw walkingSpeed addwf rightServoV addwf leftServoV return rotate_right movlw walkingSpeed subwf rightServoV subwf leftServoV return ;;;;;;;;;;;;;;;;;;;;;;;;; servo control routines ;;;;;;;;;;;;;;;;;; ; controls the servos update_roach_speed movfw leftServoV ; addwf TMR0,w ; calculate when left servo pulse should stop, movwf counter ; store in counter. bsf outputs,leftServo ; activate left servo pin, call servoTimerInnerLoop ; wait pulse length, bcf outputs,leftServo ; deactivate pin movfw rightServoV ; addwf TMR0,w ; movwf counter ; bsf outputs,rightServo ; call servoTimerInnerLoop ; send right servo its signal bcf outputs,rightServo ; movlw servoSpacer ; addwf TMR0,w ; setup and start servo pulse spacer movwf counter ; call servoTimerInnerLoop ; incf slowTimer ; acts as a slower version of TMR0 return servoTimerInnerLoop movfw counter ; if time is up, return, else wait longer subwf TMR0,w btfss status,Z goto servoTimerInnerLoop return ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; end