Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
;ANNA
; STARTED 20 September 2015
; MODIFIED 17 July 2016
__CONFIG _CP_OFF & _BODEN_OFF & _PWRTE_ON & _WDT_OFF & _XT_OSC & _LVP_OFF
list p=16f874A
#include p16f874A.inc
#include web.inc
;
;*******************************************************************************
;On start up goto setup
GOTO SETUP
ORG H'0004'
BCF INTCON,GIE ; Disnable interrupts
;Interupt service routine
;save working register contents
MOVWF W_TEMP ; Copy W to a Temporary Register
SWAPF STATUS,W ; Swap STATUS nibbles and place
MOVWF STATUS_TEMP ; Save STATUS to a Temporary register
;
BTFSC PIR1,RCIF
CALL RSCEIEVE
;
SWAPF STATUS_TEMP,W ; Swap original STATUS register value into W (restores original bank)
MOVWF STATUS ; Restore STATUS register from W register
SWAPF W_TEMP,F ; Swap W_Temp nibbles and return value to W_Temp
SWAPF W_TEMP,W ; Swap W_Temp to W to restore original W value without affecting STATUS
BSF INTCON,GIE ; Enable interrupts
service_int
RETFIE
;
SETUP
;INITIALIZING PORTA,PORTB,PORTC
BCF STATUS, RP0 ;
CLRF PORTA ; Initialize PORTA by clearing output data latches
CLRF PORTB ; Initialize PORTB by clearing output data latches
CLRF PORTC ; Initialize PORTC by clearing output data latches
;
bsf STATUS,RP0 ; Select Bank1
;
MOVLW H'0000'
MOVWF TRISA
;
MOVLW 0x00 ; Value used to initialize data direction for PORTB
MOVWF TRISB ; Set RB<3:0> as INPUT
;
MOVLW H'0080' ;set portc 6out 7in(RS232),0-3in mux data 4&5 out mux control
MOVWF PORTC ;SET UP PORT C
MOVLW h'0026' ;load setup info for usart transmit
MOVWF TXSTA ;move to uart transmit setup reg.
MOVLW D'12'
MOVWF SPBRG ;set baud rate to 19200
MOVLW H'0020' ;Enable rs232 Recieve interupt
MOVWF PIE1
bcf STATUS,RP0 ; Select Bank0
MOVLW H'0090' ;usart receive info and enable serial communication
MOVWF RCSTA ;
;
CALL WIFI_RESET
CALL MOTORS_STOP
BSF INTCON,GIE ; Enable interrupts
BSF INTCON,PEIE ; Enable interrupts RS232
BCF RS232_BIT
BCF ERROR_BIT
clrf PORTB
CLRF MOTOR_STATE
;
;****************************************************************
;RECIEVE DATA
MAIN
BTFSC RS232_BIT
CALL DECODE_RS232
;
GOTO MAIN
;
;Decode rs232 data
;
DECODE_RS232
BCF RS232_BIT
CALL MOTORS_STOP
CALL MOTOR_DELAY_LOOP
D1 ;CHECK FOR FORWARD
MOVLW 'F'
SUBWF RSDATA7,W
BTFSS ZERO
GOTO D2
MOVLW 'O'
SUBWF RSDATA6,W
BTFSS ZERO
GOTO D2
MOVLW 'R'
SUBWF RSDATA5,W
BTFSS ZERO
GOTO D2
MOVLW 'W'
SUBWF RSDATA4,W
BTFSS ZERO
GOTO D2
MOVLW 'A'
SUBWF RSDATA3,W
BTFSS ZERO
GOTO D2
MOVLW 'R'
SUBWF RSDATA2,W
BTFSS ZERO
GOTO D2
MOVLW 'D'
SUBWF RSDATA1,W
BTFSS ZERO
GOTO D2
GOTO MOTORS_FORWARD
RETURN
D2 ;CHECK FOR REVERSE
MOVLW 'R'
SUBWF RSDATA7,W
BTFSS ZERO
GOTO D3
MOVLW 'E'
SUBWF RSDATA6,W
BTFSS ZERO
GOTO D3
MOVLW 'V'
SUBWF RSDATA5,W
BTFSS ZERO
GOTO D3
MOVLW 'E'
SUBWF RSDATA4,W
BTFSS ZERO
GOTO D3
MOVLW 'R'
SUBWF RSDATA3,W
BTFSS ZERO
GOTO D3
MOVLW 'S'
SUBWF RSDATA2,W
BTFSS ZERO
GOTO D3
MOVLW 'E'
SUBWF RSDATA1,W
BTFSS ZERO
GOTO D3
GOTO MOTORS_REVERSE
RETURN
D3 ;CHECK FOR LEFT
MOVLW 'L'
SUBWF RSDATA4,W
BTFSS ZERO
GOTO D4
MOVLW 'E'
SUBWF RSDATA3,W
BTFSS ZERO
GOTO D4
MOVLW 'F'
SUBWF RSDATA2,W
BTFSS ZERO
GOTO D4
MOVLW 'T'
SUBWF RSDATA1,W
BTFSS ZERO
GOTO D4
GOTO MOTORS_LEFT
RETURN
D4 ;CHECK FOR RIGHT
MOVLW 'R'
SUBWF RSDATA5,W
BTFSS ZERO
GOTO D5
MOVLW 'I'
SUBWF RSDATA4,W
BTFSS ZERO
GOTO D5
MOVLW 'G'
SUBWF RSDATA3,W
BTFSS ZERO
GOTO D5
MOVLW 'H'
SUBWF RSDATA2,W
BTFSS ZERO
GOTO D5
MOVLW 'T'
SUBWF RSDATA1,W
BTFSS ZERO
GOTO D5
GOTO MOTORS_RIGHT
RETURN
D5 ;CHECK FOR STOP
MOVLW 'S'
SUBWF RSDATA4,W
BTFSS ZERO
GOTO D6
MOVLW 'T'
SUBWF RSDATA3,W
BTFSS ZERO
GOTO D6
MOVLW 'O'
SUBWF RSDATA2,W
BTFSS ZERO
GOTO D6
MOVLW 'P'
SUBWF RSDATA1,W
BTFSS ZERO
GOTO D6
GOTO MOTORS_STOP
RETURN
D6 ;CHECK FOR RIGHT1
MOVLW 'R'
SUBWF RSDATA6,W
BTFSS ZERO
GOTO D7
MOVLW 'I'
SUBWF RSDATA5,W
BTFSS ZERO
GOTO D7
MOVLW 'G'
SUBWF RSDATA4,W
BTFSS ZERO
GOTO D7
MOVLW 'H'
SUBWF RSDATA3,W
BTFSS ZERO
GOTO D7
MOVLW 'T'
SUBWF RSDATA2,W
BTFSS ZERO
GOTO D7
MOVLW '1'
SUBWF RSDATA1,W
BTFSS ZERO
GOTO D7
GOTO MOTORS_RIGHT1
RETURN
D7 ;CHECK FOR LEFT1
MOVLW 'L'
SUBWF RSDATA5,W
BTFSS ZERO
RETURN
MOVLW 'E'
SUBWF RSDATA4,W
BTFSS ZERO
RETURN
MOVLW 'F'
SUBWF RSDATA3,W
BTFSS ZERO
RETURN
MOVLW 'T'
SUBWF RSDATA2,W
BTFSS ZERO
RETURN
MOVLW '1'
SUBWF RSDATA1,W
BTFSS ZERO
RETURN
GOTO MOTORS_LEFT1
RETURN
;
;
;**************************************************************
;**************************************************************
;MOTOR COMMANDS
MOTORS_STOP
BSF PORTC,0
BSF PORTC,1
BSF PORTC,2
BSF PORTC,3
RETURN
MOTORS_FORWARD
BCF PORTC,0
BSF PORTC,1
BCF PORTC,2
BSF PORTC,3
RETURN
MOTORS_LEFT
BCF PORTC,0
BSF PORTC,1
BSF PORTC,2
BCF PORTC,3
RETURN
MOTORS_RIGHT
BSF PORTC,0
BCF PORTC,1
BCF PORTC,2
BSF PORTC,3
RETURN
MOTORS_REVERSE
BSF PORTC,0
BCF PORTC,1
BSF PORTC,2
BCF PORTC,3
RETURN
MOTORS_LEFT1
BCF PORTC,0
BSF PORTC,1
BSF PORTC,2
BSF PORTC,3
RETURN
MOTORS_RIGHT1
BSF PORTC,0
BSF PORTC,1
BCF PORTC,2
BSF PORTC,3
RETURN
MOTOR_DELAY
CLRF COUNT0
CLRF COUNT1
;
MOTOR_DELAY_LOOP
DECFSZ COUNT0,F
GOTO MOTOR_DELAY_LOOP
DECFSZ COUNT1,F
GOTO MOTOR_DELAY_LOOP
RETURN
;**************************************************************
;**************************************************************
WIFI_RESET
BCF WIFI_RESET_PIN
CLRF COUNT0
CLRF COUNT1
MOVLW D'8'
MOVWF COUNT2
WIFI_RESET_LOOP
DECFSZ COUNT0,F
GOTO WIFI_RESET_LOOP
DECFSZ COUNT1,F
GOTO WIFI_RESET_LOOP
DECFSZ COUNT2,F
GOTO WIFI_RESET_LOOP
BSF WIFI_RESET_PIN
RETURN
;**************************************************************
;INTERUPT VECTOR FOR RS232 RECIEVE
RSCEIEVE
BSF RS232_BIT
CLRF RSPOINTER ;CLEAR RS232 POINTER
CLRF WEB_COUNT
RECIEVE1
CLRF COUNT1
BCF RSFAIL
MOVF RSDATA6,W
MOVWF RSDATA7
MOVF RSDATA5,W
MOVWF RSDATA6
MOVF RSDATA4,W
MOVWF RSDATA5
MOVF RSDATA3,W
MOVWF RSDATA4
MOVF RSDATA2,W
MOVWF RSDATA3
MOVF RSDATA1,W
MOVWF RSDATA2
MOVF RCREG,W
MOVWF RSDATA1
INCF RSPOINTER,F
RECIEVE2
BTFSC RCBIT
GOTO RECIEVE1
DECFSZ COUNT1,F
GOTO RECIEVE2
BSF RSFAIL
MOVF RSDATA1,W
MOVWF WORK ; ECHO BACK RECEIVED VALUE
CALL RS_TRANSMIT
RETURN
;
;
;**********************************
;**********************************
;transmit content of txreg
;
RS_TRANSMIT
movf WORK,0
movwf TXREG
call rstest
return
;
;
rstest bsf 3,5
CLRF COUNT5
CLRF COUNT6
rstest1
btfsc TXBIT
goto rstest2
decfsz COUNT5,1
goto rstest1
decfsz COUNT6,1
goto rstest1
rstest2 bcf 3,5
return
;
;TIME DELAY
TIMEDELAY
MOVLW H'00FF'
MOVWF COUNT0
TDY
DECFSZ COUNT0,F
GOTO TDY
RETURN
;
;
;**************************************************************
;
END
Sub edge_range()
Dim color As Color
Dim hue As Integer = Nothing
Dim brightness As Integer
Dim x As Integer
Dim y As Integer
Dim color_hue As Color
Dim color_lum As Integer = Nothing
Dim image1 As Bitmap = Machine_vision.edges()
Dim image_color As Bitmap = Machine_vision.picture
Dim i As Integer
Dim object_location As Integer
Dim a As Integer = 0
Dim line_count As Integer = 0
Dim longest_x As Integer = 0
Dim longest_y As Integer = 0
x = 5
y = 479
For a = 0 To 62
Do Until y = 0
color = image1.GetPixel(x, y)
color_hue = image_color.GetPixel(x, y)
brightness = color.GetBrightness
If brightness = 1 Then
object_location = y
Exit Do
Else
y = y - 1
If hue = Nothing Then hue = color_hue.GetHue Else hue = (color_hue.GetHue + hue) / 2
If color_lum = Nothing Then color_lum = color_hue.GetBrightness * 10 Else color_lum = ((color_hue.GetBrightness * 10) + color_lum) / 2
End If
Loop
range(a, 0, 0) = 480 - object_location
If a + 1 <> 63 Then range(a + 1, 0, 0) = Nothing
range(a, 1, 0) = x
range(a, 2, 0) = hue
range(a, 3, 0) = color_lum
If a = 0 Then
range(a, 4, 0) = 1 : line_count = 1
range(a, 5, 0) = 0
range(a, 6, 0) = 0
ElseIf Math.Abs(range(a, 0, 0) - range(a - 1, 0, 0)) < 10 Then
range(a, 4, 0) = line_count
Else
line_count = line_count + 1
range(a, 4, 0) = line_count
End If
If 480 - object_location > longest_y Then longest_y = 480 - object_location : longest_x = range(a, 1, 0)
y = 479
x = x + 10
Next
x = 5
For a = 1 To 62
For i = 480 - range(a, 0, 0) To 479
Machine_vision.picture.SetPixel(range(a, 1, 0), i, Color.FromArgb(255, 0, 0))
Next
Next
'***********************look for wall edges **************************
Dim line_count1 As Integer = 0
Dim line_count2 As Integer = 0
y = 475
a = 0
Do Until y < 10
x = 320
color = image1.GetPixel(x, y)
color_hue = image_color.GetPixel(x, y)
vertical_lines(a, 3) = color_hue.GetHue
Do Until x < 5
color = image1.GetPixel(x, y)
color_hue = image_color.GetPixel(x, y)
hue = color_hue.GetHue
color_lum = color_hue.GetBrightness * 10
color_hue = image_color.GetPixel(x, y)
brightness = color.GetBrightness
If brightness = 1 Then
Exit Do
Else
If hue = Nothing Then hue = color_hue.GetHue Else hue = (color_hue.GetHue + hue) / 2
If color_lum = Nothing Then color_lum = color_hue.GetBrightness * 10 Else color_lum = ((color_hue.GetBrightness * 10) + color_lum) / 2
x = x - 1
End If
Loop
vertical_lines(a, 0) = 480 - y
vertical_lines(a + 1, 0) = Nothing
vertical_lines(a, 1) = 320 - x
If a = 0 Then
vertical_lines(a, 5) = 1 : line_count1 = 1
vertical_lines(a, 6) = 0
vertical_lines(a, 7) = 0
ElseIf Math.Abs(vertical_lines(a, 1) - vertical_lines(a - 1, 1)) < 10 Then
vertical_lines(a, 5) = line_count1
Else
line_count1 = line_count1 + 1
vertical_lines(a, 5) = line_count1
End If
'***********************
x = 320
Do Until x > 635
color = image1.GetPixel(x, y)
'color_hue = image_color.GetPixel(x, y)
brightness = color.GetBrightness
If brightness = 1 Then
Exit Do
Else
If hue = Nothing Then hue = color_hue.GetHue Else hue = (color_hue.GetHue + hue) / 2
If color_lum = Nothing Then color_lum = color_hue.GetBrightness * 10 Else color_lum = ((color_hue.GetBrightness * 10) + color_lum) / 2
x = x + 1
End If
Loop
vertical_lines(a, 2) = x - 320
vertical_lines(a, 3) = hue
vertical_lines(a, 4) = color_lum
If a = 0 Then
vertical_lines(a, 8) = 1 : line_count2 = 1
vertical_lines(a, 9) = 0
vertical_lines(a, 10) = 0
ElseIf Math.Abs(vertical_lines(a, 2) - vertical_lines(a - 1, 2)) < 10 Then
vertical_lines(a, 8) = line_count2
Else
line_count2 = line_count2 + 1
vertical_lines(a, 8) = line_count2
End If
a = a + 1
y = y - 10
Loop
'*****************************************
For a = 1 To 96
If vertical_lines(a, 0) = Nothing Then Exit For
For i = 320 - vertical_lines(a, 1) To vertical_lines(a, 2) + 320
Machine_vision.picture.SetPixel(i, 480 - vertical_lines(a, 0), Color.Blue)
Next
Next
Machine_vision.PictureBox1.Image = Machine_vision.picture
End Sub
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.