Wednesday, June 25, 2014

Pennybot pic16f628 asm code

The full pennybot asm code..

include "P16F628A.INC" ;include the defaults for the chip
list p=16f628a
__config (_CP_OFF & _CPD_OFF & _PWRTE_OFF & _WDT_OFF & _INTRC_OSC_NOCLKOUT & _LVP_OFF & _BOREN_OFF & _MCLRE_OFF)

; pin out list
; PORTA RA
; 0 comparator input - right line sensor
; 1 comparator input - left line sensor
; 2 comparator input  - reference voltage
; 3 motor driver output A - right motor
; 4 motor driver output A - right motor
;                       - open collector - needs pull up resistor to 5V to go high
; 5 unused - input only
; 6 motor driver output B - left motor
; 7 motor driver output B - left motor

; PORTB RB
; 0 IR detector input - front - IR detectors go low on signal, normally 5V
; 1 unused
; 2 status led - blinks while code in main loop
; 3 digital output, PWM
; 4 IR detector input - left  front  - IR detectors go low on signal, normally 5V
; 5 IR detector input - right front
; 6 IR detector input - left side
; 7 IR detector input - right side

; general variables
cblock 0x20
count ; used in delay routine
count1 ; used in delay routine
counta ; used in delay routine
countb ; used in delay routine
        compcount   ; used in comparator compare
endc

#define W_TEMP  0x70 ; common memory area across all banks
#define STATUS_TEMP 0x71 ; temp vars for ISR

org 0x00
goto init

;
; ISR code
;
org 0x04
movwf   W_TEMP ; Save W & STATUS, etc
swapf   STATUS, W
bcf     STATUS, RP0
movwf   STATUS_TEMP ; end ISR save bits

btfsc PIR1, CMIF
call check_comp     ;check comparators - line detection
btfsc INTCON,INTF
    goto check_front_IR    ;check single front IR detector, skip other IR decectors
btfsc INTCON,RBIF
call check_IR       ;check other IR detectors
isr_exit
swapf   STATUS_TEMP, W   ; restore W & STATUS
movwf   STATUS
swapf   W_TEMP,F
swapf   W_TEMP,W
retfie                  ; end ISR restore bits

; end ISR code

init
; init steps are
                ; setup PORTA and PORTB pins as input/output
; halt motors
; delay 5 seconds
; setup comparator mode
; setup ir detectors - done as enable interrupt
; setup rear ir sensor
; setup 38khz, 50% duty cycle PWM
; enable interrupts


banksel TRISA ; set all pins in appropriate input/output
movlw b'00100111' ; RA5 always input
movwf TRISA ; set PORTA pins
movlw b'11110001'
movwf TRISB ; set PORTB pins

    call stop           ; motors halt

    banksel PORTB
    bsf PORTB,2         ; turn on test led

call Long_Delay ; delay 5-6 secs

banksel CMCON ; init comparator mode
movlw 0x03
    movwf CMCON ; CM<2:0> = 011
    call Delay10        ; delay 10 microseconds
    movf CMCON,F ; read cmcon to end change condition wrt interrupt setup
    bcf PIR1, CMIF ; clear pending interrupts
    banksel PIE1 ; select bank 1
    bsf PIE1, CMIE ; enable comparator interrupts

            ; setup and run 38khz pwm
banksel PR2         ; set pwm period
movlw   0x19 ; 0x19 == 25 decimal.  So via equation
movwf   PR2         ; 25+1 * 4 * 0.25 * 1 = 26 and 1/26 == 38khz ish
                        ; set pwm duty cycle to 50%
    banksel CCP1CON ; so need duty cycle to be half pwm period
            ; ie half (25+1)*4 -> 52 = 0x34
    movlw   0x0c ; since Tosc is in both pwm and duty cycle equation they cancel out
    movwf   CCP1CON ; remember split across bits 5/4 of ccp1con and ccpr1l
    movlw   0x0d ;  0x   0    d  0
    movwf   CCPR1L   ; duty cycle 0x34 - ie 52 decimal
    banksel T2CON ; set tmr2 prescaler
    movlw   0x00
    movwf   T2CON
    bsf T2CON, TMR2ON ; turn on timer 2


banksel INTCON ; enable interrupts
    bsf INTCON, RBIE    ; enable RB port interrupts (RB4-7)
    bsf INTCON, INTE    ; enable RB0 port interrupt
    bsf INTCON, PEIE ; enable peripheral interrupts
    bsf INTCON, GIE ; enable global interrupts

main
nop
    banksel PORTB
    bcf PORTB,2         ; turn off status led
call forward
goto main

;
; check front IR detector, go forward if something there
;
check_front_IR
    call Delay20        ; wait for signal to settle
    banksel PORTB
    btfss   PORTB,0     ; check RB0, low means we detected something
    call    forward   ; move ahead
    banksel INTCON
    bcf     INTCON,INTF    ; clear RB0 interrupt flag (maybe clear other IR interrupt flag?)
    goto isr_exit

; end check_front_IR

;
; check what IR detetor went off and change motors as needed
; low means we detected something
;

check_IR
    call Delay20        ; wait for signals to steady
    banksel PORTB
    btfss PORTB,6       ; check right side
    goto turnright_IR   ; turn right as something was there
    btfss PORTB,7       ; check left side
    goto turnleft_IR    ; turn left as something was there
    btfss PORTB,5       ; check left front
    call frontleft_IR
    btfss PORTB,4       ; check right front
    call frontright_IR
checkirexit
    banksel INTCON
    bcf INTCON,RBIF     ; clear interrupt flag
    return
turnright_IR
call fastright
goto checkirexit
turnleft_IR
call fastleft
goto checkirexit
frontright_IR
call slowright
goto checkirexit
frontleft_IR
call slowleft
goto checkirexit

; end check_IR



;
; check comparators function (line sensors)
; reverse if both sensors are active
check_comp
call Delay20        ; wait 20ms for input to get stable
banksel CMCON
btfsc CMCON,C1OUT   ; if c1out is 1
    bsf compcount,0     ; got a right sensor reading
    btfsc CMCON,C2OUT   ; if c2out is 1
    bsf compcount,1     ; got a left sensor
    movlw   d'3'        ; compcount is 0000 0011 if both sensors active ie 3
    subwf   compcount,0
    btfsc   STATUS,Z    ; was the result 0?
    goto comp_reverse   ; both sensors active, reverse, otherwise check again
btfsc CMCON,C1OUT   ; right sensor active
    goto comp_turnleft      ; turn right
    btfsc CMCON,C2OUT   ; left sensor active
    goto comp_turnright      ; turn left
compexit
bcf PIR1, CMIF      ; clear pending interrupts
return
comp_reverse
    call reverse        ; both sensors active, reverse
    call fastleft
goto compexit
comp_turnleft
call fastleft
goto compexit
comp_turnright
call fastright
goto compexit

; end check_comp

; motor drive functions

;
; halt both motors by setting both outputs high
;
stop
banksel PORTA
movlw b'11011000'
movwf PORTA
return

;
; forward on both motors
;
forward
banksel PORTA
movlw b'01001000'
movwf PORTA
    ;call    Delay255
    call    Delay50
return

;
; reverse on both motors
;
reverse
banksel PORTA
movlw b'10010000'
movwf PORTA
    call    Delay255
return

;
; turn right on spot
;
fastright
banksel PORTA
movlw b'10001000'
movwf PORTA
    call    Delay255
return

;
; turn right by stopping left motor
;
slowright
banksel PORTA
movlw b'11001000'
movwf PORTA
    call    Delay255
return

;
; turn left on the spot
;
fastleft
banksel PORTA
movlw b'01010000'
movwf PORTA
    call    Delay255
return

;
; turn left by stopping right motor
;
slowleft
banksel PORTA
movlw b'01011000'
movwf PORTA
    call    Delay255
return

; end motor functions

;
; Delay routines (based on 4MHz clock)
; http://www.piclist.com/techref/piclist/codegen/delay.htm

Long_Delay
        movlw  d'60'       ;delay 6 seconds
call Delay100W
return

Delay100W movwf count ;delay W x 100mS
d2 call Delay100 ;maximum delay 25.5 seconds
decfsz count ,f
goto d2
return

Delay255 movlw 0xff ;delay 255 mS
goto d0
Delay100 movlw d'100' ;delay 100mS
goto d0
Delay50 movlw d'50' ;delay 50mS
goto d0
Delay20 movlw d'20' ;delay 20mS
goto d0
Delay10 movlw d'10' ;delay 10mS
goto d0
Delay1 movlw d'1' ;delay 1mS
goto d0
Delay5 movlw 0x05 ;delay 5.000 ms (4 MHz clock)
d0 movwf count1
d1 movlw 0xC7      
movwf counta
movlw 0x01      
movwf countb
Delay_0 decfsz counta, f
goto $+2
decfsz countb, f
goto Delay_0

decfsz count1 ,f
goto d1
return

;end of Delay routines

end

Saturday, June 21, 2014

Pennybot finally makes it to the big time - sumobot finals


In order to get pennybot into the winner’s circle I needed to make a number of changes…

Realising that the rear IR sensor wasn’t really useful I unsoldered it and added it to the front of pennybot as a front sensor between the current left/right sensors.  This sensor was connected to the interrupt pin RB0.

I made a new metal scoop for the front with the bottom edge formed around a piece of wire for extra strength.  This scoop just scrapes the floor.  Great for lifting up opponents but terrible on the sumo ring paint job.

I went back to a much simpler code base for the IR detection function.  Firstly in the ISR section I check each interrupt flag to see what interrupt was triggered before calling that function.  Before I was running through all the sensor functions (line sensor/IR detectors) regardless.  Now with the direct front IR sensor being to RB0 I had three functions.  Firstly check the line sensors as staying within the ring is the most important behaviour.  Second check the direct front IR sensor (RB0) and go forward if that detects.  Once in this function skip the rest of the IR sensor checks before exiting the ISR code section.  This is to stop a false hit from making pennybot go left/right after a forward move.  Finally check the other IR sensors on RB4-7.

Also I removed all the loop and repeat checks.  Rather we go through the ISR code once and then out.  Previously I was getting stuck in function call loops that were going more than 8 deep and thus breaking as the stack would loop over itself.

Finally I changed the motor driving code for when there was a left front or right front detection.  Rather than turn on the spot we now do a slow right or left turn by braking one wheel (by making both motor inputs high) and only powering the other.  This results in a turn with the pivot point being the braked wheel rather than the centre of pennybot.

In previous bouts dumbbot was the winner 4 out of 5 matches.  The push strength dumbbot has was too much and it’s wheels too grippy for pennybot to push.  Now it was a different story.  Pennybot won every single match.  The new IR detection code worked great, the slow turns allow pennybot to track much better and the new scoop lifts up dumbbot slightly so it loses traction and can be pushed back.  Finally success.

The only minor points I’m considering to fix is the turn time on the line sensors.  Sometimes when there is line detection pennybot turns so far on the spot the other line sensor detects and we end up spinning on the spot.  The third IR emitter circuit (previously used for the rear IR emitter) is still on the board but unused.  Perhaps I will unsolder it but it seems more effort than it is worth.  Also I should add in a watchdog timer to get me out of unknown code failures.  The watchdog timer is one part of the pic programming I haven’t played with yet.

It’s been a long time but finally I can have a sumo battle.

pic16f628 and USART via a max232 chip

Max232 chip and associated capacitors.  3.5mm jack on the right.

While I was thinking out pennybot I was wondering how I could get more debug information out as the single blinking led I have currently isn’t much.  I remembered that the pic16f628 had usart onboard so a read up on it.  Only two pins were needed with a max232 chip to handle the voltage pumping to a RS232 connection. Sounded pretty easy.

However before this point I needed a usb to serial dongle and a serial terminal program for my Mac.  I already had a usb/serial adapter based on the pl2302 chip, a cheap ebay purchase.  I searched around and found the necessary drivers for Mac so the pl2302 would be detected.  Google is your friend for that.  For a terminal program some people suggested using the ‘script’ command but that didn’t work very well.  I then tried CoolTerm which was great and I fully recommend it.

For the max232 chip (and associated capacitors) I knew that I would want to use it both on my breadboard and possibly on future pic projects.  A standalone board was the best option.  I had a 4 pin (5v, GND, RX, TX) male header to connect the board to a breadboard or circuit and a stereo 3.5mm socket to connect the serial cable to.  The serial cable was a recycle job were I got an existing 9 pin cable, cut off one end and wired up RX, TX and GND to a 3.5mm male stereo plug.  It’s smaller than a full 9pin shell and is quite robust.  Now I will be reserving the USART pins on pic projects in future and having a 4 pin female header so I plug in my max232 board when needed.