; this is for operating bump sensors
;
;  sensors:
;   RA2  LEFT Bump
;   RA3  RIGHT Bump
;   RA4  STOP switch

 ERRORLEVEL -302  ; shut the hell up about whitespace

LIST P=P16F627         ; gpasm

include "p16f627.inc" ;gpasm 
include "misc.inc"   ;gpasm
 
; __CONFIG CONFIG & _ER_OSC_CLKOUT  ; gpasm
 __CONFIG CONFIG & _XT_OSC  ; gpasm


;config WDT=off, OSC=HS ; HS,XT,LP,RC  picasm

; ------------ Number definitions --------------

; ------------- Memory locations ---------------
;
;Value             equ 0x0C  ; value being transfered


;============================= MAIN ================================
;
; Copy values from port A and send them to port B
;


 CBLOCK RAM
 
   DelayCtr1  ; ram locations for delay counters
   DelayCtr2
   DelayCtr3
   
   Random     ; 'random' decision
   
   Sensors    ; memory location for sensors
   
 ENDC


InitPortADir   equ b'00011111'  ; PORT A all bits are not set to output  ;-)
InitPortBDir   equ b'00000000'  ; PORT B all bits output 
InitPortAState equ b'00000000'  ; 
InitPortBState equ b'11111111'  ; 



;-------Macros        

Bank0 macro				;macro to select bank 0
   bcf    STATUS, RP0  ; select bank 0
 endm
        
Bank1 macro				;macro to select bank 1
  bsf    STATUS, RP0  ; select bank 1
 endm

  ;---- movement macro's

GoForward macro
  bsf PORTB, 0 ;GPturn the left motor on 
  bcf PORTB, 1 ;
  bsf PORTB, 2 ; turn the right motor on
  bcf PORTB, 3 ;
 endm
  
GoRight macro
  bcf PORTB, 0 ;stop the left motor from going forward
  bsf PORTB, 1 ;turn the left motor on in reverse
  bsf PORTB, 2 ;turn the right motor on
  bcf PORTB, 3 ;ensure the right motor is not in reverse or breaking
  endm

GoLeft  macro
  bsf PORTB, 0 ;turn the left motor on going forward
  bcf PORTB, 1 ;ensure the left motor is not going in revers or brakeing
  bcf PORTB, 2 ;turn the right motor off in forward
  bsf PORTB, 3 ;turn the right motor on in reverse
 endm

Stop   macro ; Sets all outputs to low
  bcf PORTB, 0 
  bcf PORTB, 1
  bcf PORTB, 2
  bcf PORTB, 3
 endm  

Reverse  macro; Will set only the reverse output bits high
  bcf PORTB, 0 
  bsf PORTB, 1
  bcf PORTB, 2
  bsf PORTB, 3
 endm

Reverse_R macro;will make a reverse turn to the right
  bcf PORTB, 0 
  bcf PORTB, 1
  bcf PORTB, 2
  bsf PORTB, 3
 endm

Reverse_L macro; will make a reverse turn to the left
  bcf PORTB, 0 
  bsf PORTB, 1
  bcf PORTB, 2
  bcf PORTB, 3
 endm

LeftLEDOn macro ; turn on only the left led
  bcf PORTB, 5
  bsf PORTB, 4  
 endm

RightLEDOn macro ; turn on only the right led
  bsf PORTB, 5
  bcf PORTB, 4  
 endm

LEDsOff macro ; turn both leds off
  bcf PORTB, 5
  bcf PORTB, 4
 endm




;-------------

 ORG 0x00

_main:

  bcf    INTCON, GIE  ; disable interrupts
  movlw  0x07         ; disable comparators
  movwf  CMCON
  
  Bank0
    
  movlw  InitPortAState ; Write the byte containing the value zero as defined in InitPortAState to port a
  movwf  PORTA
  
  movlw  b'00000111'  ; comparitors off
  movwf  CMCON        
  
  movlw  InitPortBState
  movwf  PORTB
  
  Bank1
  
  movlw  InitPortBDir         ; all port B as output
  movwf  TRISB
  
  movlw  InitPortADir         ; the first the least most bits are set to input on portB
  movwf  TRISA
  
  Bank0
  
  LEDsOff
  call Delay3
  
  
_mainLoup:  
  
 incf Random, F 
  
 ; to test the sensors,
 LEDsOff
 call Delay1
 
 ; wait for the sensor to go high
 btfss PORTA, 2
   goto $-1
 
 ; turn the right LED on
 RightLEDOn 
 ; wait
 call Delay1
 ; read the sensor and store the result in the ram location
 btfsc PORTA,2
   bsf Sensors, 3
 btfss PORTA, 2
   bcf Sensors, 3

   
 ; to test the sensors,
 LEDsOff 
 call Delay1
 ; wait for the sensor to go high
 btfss PORTA, 2
   goto $-1
 ; turn the left LED on
 LeftLEDOn
 ; wait
 call Delay1
 ; read the sensor and store the result in the ram location
 btfsc PORTA,2
   bsf Sensors, 2
 btfss PORTA, 2
   bcf Sensors, 2
 
 ; turn off all the sensors
 LEDsOff 
  
 ;   RA2  LEFT
 ;   RA3  RIGHT


test1:
 btfsc Sensors, 2 ;left bump sensor?
   goto test2
 btfss Sensors, 3 ;right bump sensor also?
   goto BothSensors
 ; at this point we only have the left sensor
 GoRight 
 goto _mainLoup ; go back to start of tests  


test2:  
 btfsc Sensors, 3 ;right bump sensor?
   goto test3  ; if not skip the following code
 GoLeft
 goto _mainLoup
   
test3:
  GoForward
  
    
 goto     _mainLoup

BothSensors:
  Stop
  call Delay1
  Reverse
  call Delay3
  btfss Random, 0 
    goto bla
  Reverse_R
bla:
  btfsc Random, 0
    goto lkj
  Reverse_L
lkj:
  call Delay3
  GoForward
 ; clrf Random   ; bug, always turns the same way after collision
  goto _mainLoup



 ;========================| SUBROUTINES |==============================

; delay of 65536

Delay3:   
   movlw    D'5'       ; set a delay of 256
   movwf    DelayCtr3
DelayLoup3:
   call     Delay1
   decfsz   DelayCtr3, F 
      goto  DelayLoup3   
 return
 
 
Delay1:   
   clrf     DelayCtr1  ; set a delay of 256
   decf     DelayCtr1, F
DelayLoup:
   call     Delay2
   decfsz   DelayCtr1, F 
      goto  DelayLoup   
 return


;delay of 256

Delay2:
   clrf     DelayCtr2  ; set a delay of 256
   decf     DelayCtr2, F
DelayLoup2:
   decfsz   DelayCtr2, F 
      goto  DelayLoup2   
 return


; end of source
end