; ***********************************************************************************************************************************************
; This is a program that was written by 3 university-students (Sybren Van der Straeten, Branko Brackx and Ward De Paepe) for a class called "Mechatronics" at the Vrije Universiteit Brussel, Brussels, Belgium.
; The program is written for use on a PIC16F877 microcontroller and is quite application specific.
; This program or parts of it may be used freely but please, refer to us properly.
; For more information about what the program does, please refer to our website (somewhere on http://mech.vub.ac.be/)
; ***********************************************************************************************************************************************
LIST p=16f877a ; List directive to define processor
INCLUDE "P16F877A.inc" ; Processor specific variable definitions
__CONFIG _CP_OFF & _DEBUG_OFF & _LVP_OFF & _PWRTE_ON & _WDT_OFF & _BODEN_OFF & _HS_OSC
; General memory allocation
X_Pos EQU h'20'
X_Goal EQU h'21'
Y_Pos EQU h'22'
Y_Goal EQU h'23'
Hoek EQU h'24' ; "Hoek" is the Flemish word for "Angle"
Uitl_L EQU h'25'
Uitl_R EQU h'26'
; Uitl_L en Uitl_R are the voltages coming from the SHARP-IR-sensors, after A/D-conversion
Kleinste EQU h'27' ; "Kleinste" means "Smallest"
Grootste EQU h'28' ; "Grootste" means "Largest"
Afstand_Links EQU h'29' ; "Afstand" means "Distance" and "Links" means "Left"
Afstand_Rechts EQU h'2A' ; "Rechts" means "Right"
Loodr_Afst_Links EQU h'2B'
Loodr_Afst_Rechts EQU h'2C'
SchietHoek EQU h'2D'
HoekEersteKwadr EQU h'2E'
Nul EQU h'2F'
LinksKort EQU h'30' ; "Kort" means "Short"
LinksVer EQU h'31' ; "Ver" means "Far"
RechtsKort EQU h'32'
RechtsVer EQU h'33'
RES_1_4 EQU h'34'
RES_2_4 EQU h'35'
RES_2_3 EQU h'36'
X_1 EQU h'37'
LinksGroterDanRechts EQU h'38'
X_2 EQU h'39'
TEMP EQU h'3A'
SchuineZijde EQU h'3B'
TEMP_ABS_A EQU h'3C'
TEMP_ABS_B EQU h'3D'
GrijsA EQU h'40' ; "Grijs" means "Grey" (refers to the greyscale on the floor)
GrijsB EQU h'41'
GrijsC EQU h'42'
WelkePoort EQU h'43'
Gemiddelde EQU h'44'
AGroterDanB EQU h'45'
AGroterDanC EQU h'46'
BGroterDanC EQU h'47'
TEMP1 EQU h'48'
TEMP2 EQU h'49'
TEMP3 EQU h'4A'
TEMP4 EQU h'4B'
GewensteHoek EQU h'50'
HoekTolerantie EQU h'51'
ActionL EQU h'52'
ActionR EQU h'53'
Timer_local EQU h'54'
IR EQU h'55'
LastIR EQU h'56'
; IR1, IR2 and IR3 represent bit-values on port B and are all allocated in the same bite IR : IR<1> = IR1, IR<2> = IR2, IR<3> = IR3
; IR 1 = comparation of the two IR-sensors in front (1 = ball is to the left)
; IR 2 = comparation of the two IR-sensors in the back (1 = ball is to the left)
; IR 3 = threshold value for the sum of the two IR-sensors in front is exceeded (1) or not (0)
Timer_interrupt EQU h'57'
PCLATH_TEMP EQU h'58'
W_TEMP EQU h'59'
STATUS_TEMP EQU h'5A'
RES_XOR EQU h'5B'
TMR1L_END EQU H'5C'
TMR1H_END EQU H'5D'
Afst_Obj EQU h'5E'
Ping_Counter EQU h'5F'
Zoek_Counter EQU h'70'
; Memory allocation for the math library
#define PRECISION 2 ; byte size for registers --> 2 means the math library works with 16-bit numbers
M_STOR_STATUS macro WHERE
movf STATUS,w
movwf WHERE
endm
M_RETR_STATUS macro WHERE
movf WHERE,w
movwf STATUS
endm
cblock 0x60
REG_X:PRECISION
REG_Y:PRECISION
REG_Z:PRECISION
REG_COUNTER
REG_STATUS
REG_T1
REG_T2
REG_ROT_COUNTER
endc
ORG H'00'
GOTO MAIN
ORG H'04'
GOTO ISR
START
MAIN
; "Initialisation of the PWM's"
BSF STATUS,RP0 ; go to Bank 1
BCF TRISC,0 ; RC0 Output for PWM2 (rightside motor)
BCF TRISC,1 ; PWM2 Output (rightside motor)
BCF TRISC,2 ; PWM1 Output (leftside motor)
BCF TRISC,3 ; RC3 Output for PWM1 (leftside motor)
MOVLW H'FF'
MOVWF PR2 ; largest possible period for the PWM's
BCF STATUS,RP0 ; go to Bank 0
MOVLW H'80' ;
MOVWF ActionL
MOVWF ActionR
Call SetSpeed ; Both PWM's are now initialized with zero speed
MOVLW B'00000110' ; Prescaler 16 en TMR2 'on'
MOVWF T2CON
MOVLW B'00001100' ; PWM1-mode 'on' (leftside motor)
MOVWF CCP1CON
MOVLW B'00001100' ; PWM2-mode 'on' (rightside motor)
MOVWF CCP2CON
; "Initialisation of the ports"
BSF STATUS, RP0 ; go to Bank 1
BSF TRISA, 0 ; RA0 Input (GrijsA)
BSF TRISA, 1 ; RA1 Input (GrijsB)
BSF TRISA, 3 ; RA3 Input (GrijsC)
BCF TRISC, 4 ; RC4 Output (werkingsLEDje)
BCF TRISC, 7 ; RC7 Output (Debugging-purposes)
BSF TRISB, 0 ; RB0 Input (On/Off-switch)
BSF TRISB, 1 ; RB1 Input (Black/White-switch)
BCF TRISB, 3 ; RB3 Output (dribbler)
BSF TRISB, 4 ; RB4 Input (IR1)
BSF TRISB, 5 ; RB5 Input (IR2)
BSF TRISB, 6 ; RB6 Input (IR3)
BCF TRISB, 7 ; RB7 Output (PING-gate is initially an output)
BSF TRISE, 6 ; RE6 Input (SHARP left)
BSF TRISE, 7 ; RE7 Input (SHARP right)
BCF STATUS, RP0 ; go to Bank 0
;; "Initialisation Timer0"
;; banken!!!
; BCF OPTION_REG, T0CS ; timer in stead of counter
; BCF OPTION_REG, PSA ; prescaler used for TMR0
; BSF OPTION_REG, PS2
; BSF OPTION_REG, PS1
; BSF OPTION_REG, PS0 ; this sets the prescaler to 1/256
; "Initialisation A/D-convertor"
BSF STATUS, RP0 ; go to Bank 1
MOVLW B'00000000' ; all possible analog inputs enabled, Fosc/2, left justified
MOVWF ADCON1 ;
BCF STATUS, RP0 ; go to Bank 0
MOVLW B'00000001' ; AD module powered on, read in channel nul (AN0), Fosc/2
MOVWF ADCON0 ;
; "Initialisation Timer1"
BCF STATUS,RP0 ; go to Bank 0
MOVLW B'00000001' ;
MOVWF T1CON ; T1OSCEN 0 TMR1CS 0 TMR1ON 1
; We just set TMR1 for internal clocksignal (Tosc), "do not synchronize" and "on"
; fosc => 4 MHz/4 = 1 MHz => the counter will increase every 1 microsecond (= 1 cycle time)
; "Initialisation of the program"
BCF STATUS, RP0 ; goto Bank 0
MOVLW h'57'
MOVWF X_Goal ; Length of the field is 87 cm * 2 bits / cm = 174 bits; the goal is at length/2 --> d'87' = h'57'
MOVLW h'EE'
MOVWF Y_Goal ; Width of the field is 119 cm * 2 bits / cm = 238 bits; the goal is at width
MOVLW d'0'
MOVWF Zoek_Counter ; initialise Zoek_Counter at 0
; *****************************************************************************************************************************************
; ********************************************** Main program ***************************************************
; *****************************************************************************************************************************************
LOOP_Main
BSF PORTB, 3 ; Dribbler On
Call StilLeggen
Call ZoekBal
GOTO LOOP_Main
; "ZoekBal"
; Subroutine to localize the ball (in a radial sense) and turns roughly towards it (fast)
ZoekBal
LOOP_ZoekBal
BTFSC IR, 3 ; IR3 is set if a certain threshold is crossed (threshold chosen so that IR3 is set if the ball is "in front" of the robot)
GOTO RegelZoek ; IF IR3 = 1 THEN go to RegelZoek
; ELSE we will have to turn the robot some more
BTFSC IR, 2
Call DraaiNaarLinks ; IF IR2 = 1 THEN ball is at the left of the robot and we will have to turn the robot to the left
BTFSS IR, 2
Call DraaiNaarRechts ; IF IR2 = 0 THEN ball is at the right of the robot and we will have to turn the robot to the right
Call IRsInlezen ; After a turn has been made, the IR-sensor-readings have to be refreshed
GOTO LOOP_ZoekBal
; "RegelZoek"
; Subroutine that localizes the ball (in a radial sense) with more accuracy (slower)
; It will end when the ball is exactly in front of the robot
RegelZoek
MOVF IR, 0 ; IR to W
MOVWF LastIR ; IR to LastIR
LOOP_RegelZoek
BTFSS IR, 3
GOTO ZoekBal ; IF IR3 = 0 THEN the ball is no longer in front of the robot and we will have to look for it again
MOVF IR, 0 ; IR to W
XORWF LastIR, 0 ; Bitwise Exclusive OR of IR with LastIR, result placed in W; truth table of XOR: 00>0, 11>0, 01>1 en 10>1
MOVWF RES_XOR ; Because a bit test is not always possible on W
BTFSC RES_XOR, 1 ; If IR1 changed (i.e. if XOR = 1), then the ball is right in front of the robot and we will continue to the next SR
GOTO NaarBal
BTFSC IR, 1 ; If IR1 did not change (i.e. XOR = 0), then the ball is not right in front of the robot and a next turn will have to be made
Call DraaiBeetjeNaarLinks ; IF IR1 = 1 THEN Ball is to the left of the robot and we will have to turn the robot to the left
BTFSS IR, 1
Call DraaiBeetjeNaarRechts ; IF IR1 = 0 THEN Ball is to the right of the robot and we will have to turn the robot to the right
MOVF IR, 0 ; IR to W
MOVWF LastIR ; IR to LastIR
Call IRsInlezen ; After a turn has been made, the IR-sensor-readings have to be refreshed
GOTO LOOP_RegelZoek
; "NaarBal"
; Subroutine that advances the robot (probably towards the ball, see RegelZoek from which we came to the SR)
NaarBal
Call IRsInlezen ; Refresh the IR-sensor-readings
BTFSS IR, 3
GOTO ZoekBal ; IF IR3 = 0 THEN the ball is no longer in front of the robot and we will have to look for it again
BTFSC IR, 1
Call VooruitEnNaarLinks ; IF IR1 = 1 THEN the ball is slightly to the left of the robot and we will advance the robot and turn it slightly to the left
BTFSS IR, 1
Call VooruitEnNaarRechts; ; IF IR1 = 1 THEN the ball is slightly to the right of the robot and we will advance the robot and turn it slightly to the right
; Now we will read the ultrasonic sensor and compare it to a value of which we know that it corresponds with "we are very close to the ball"
Call Ping
MOVLW d'12' ; 12 bits equals 6 cm (2 bits / cm)
SUBWF Afst_Obj,0 ; Afst_Obj is the result of the function Ping and represents the distance to the object in front of the robot, 2 bits / cm
BTFSS STATUS, C
GOTO NeemBal ; IF C = 0 THEN a borrow happened and Afst_Obj<12 so we are very close to the ball
GOTO NaarBal ; IF C = 1 THEN no borrow happened and Afst_Obj>12 so we will have to move still closer to the ball
; "NeemBal
; Subroutine that quickly advances towards the ball in a straight line (we know the ball is right in front AND very close so there is but very little chance we will miss it, plus a high speed is needed for the dribbler to capture the ball)
NeemBal
MOVLW h'BF'
MOVWF ActionL
MOVLW h'BF'
MOVWF ActionR
Call SetSpeed ; Move quickly towards the ball
MOVLW d'250'
Call WaitNms
MOVLW d'250'
Call WaitNms
Call StilLeggen ; We should now have captured the ball so we can afford a standstill
; Now we will turn towards the goal
Call DraaiNaarGoal
; Now we just go straight ahead in the direction of the goal and score
MOVLW h'DF'
MOVWF ActionL
MOVLW h'DF'
MOVWF ActionR
Call SetSpeed ; Moving quickly in a straight line
Call Wait1Second
Call Wait1Second
Call Wait1Second
Call Wait1Second
Call Wait1Second
Call Wait1Second
Call Wait1Second
Call Wait1Second
; We probably should have scored by now
Call StilLeggen ; Put the robot in stand-still
BCF PORTB, 3 ; Dribbler Off
sleep ; End of the program
; *****************************************************************************************************************************************
; ********************************************** DraaiNaarGoal ***************************************************************
; *****************************************************************************************************************************************
; Subroutine that aims the robot towards the goal. In order to do this, the robot has to know where it is localized and where the goal is localized.
DraaiNaarGoal
; Initialisation of the position of the goal
MOVLW d'238'
MOVWF Y_Goal
MOVLW d'87'
MOVWF X_Goal
MOVLW D'0' ; Zero to W
MOVWF Nul ; Nul set to Zero (used in SR HoekNaarEersteKwadrant)
MOVLW d'20'
MOVWF HoekTolerantie ; The tolerance of the angular position to be attained is 8 256ths of a circle
; The next code will fill in X_Pos, Y_Pos and Hoek ("Angle"), i.e. the position and orientation of the robot
Call GRIJS_NAAR_HOEK ; Uses the grey values on the floor to fill in Y_Pos and Hoek
Call UITL_NAAR_AFSTAND_X_POS_NIEUWE_SHARP ; Uses the SHARP IR's to fill in X_Pos
; The next code will determine SchietHoek i.e. the angle at which the robot sees the goal
; Comment not translated in English
MOVF X_Pos, 0 ; X_Pos naar W
SUBWF X_Goal ; W := X_Goal - X_Pos
BTFSS STATUS, C
GOTO RechterKantVeld ; Als B = C = 0 dan was X_Pos > X_Goal en zitten we langs de rechterkant van het veld
; LinkerKantVeld ; Als B = C = 1 dan was X_Pos < X_Goal en zitten we langs de linkerkant van het veld
MOVWF REG_X ; X_Goal-X_Pos naar REG_X (LSB)
MOVLW d'34'
MOVWF REG_Y ; 34,3 naar REG_Y (LSB)
Call M_MUL ; REG_Z (LSB en MSB) bevatten nu 34,3* (X_Goal - X_Pos)
MOVF Y_Pos, 0 ; Y_Pos to W
SUBWF Y_Goal, 0 ; W := Y_Goal - Y_Pos
MOVWF REG_X ; REG_X bevat nu (Y_Goal - Y_Pos)
Call M_DIV ; REG_Y (normaal enkel LSB) bevat nu de gecomplementeerde van de SchietHoek in Bits
MOVF REG_Y, 0 ; REG_Y (LSB) to W
SUBLW h'00' ; W bevat nu 00 - W maw het inverse --> de gecomplementeerde hoek!
MOVWF SchietHoek
GOTO VOOR_MIKKEN
RechterKantVeld
MOVF X_Goal, 0 ; X_Goal to W
SUBWF X_Pos ; W := X_Pos - X_Goal
MOVWF REG_X ; X_Pos - X_Goal to REG_X (LSB)
MOVLW d'34'
MOVWF REG_Y ; 34,3 to REG_Y (LSB)
Call M_MUL ; REG_Z (LSB en MSB) bevatten nu 34,3* (X_Pos - X_Goal)
MOVF Y_Pos, 0 ; Y_Pos to W
SUBWF Y_Goal, 0 ; W := Y_Goal - Y_Pos
MOVWF REG_X ; REG_X bevat nu (Y_Goal - Y_Pos)
Call M_DIV ; REG_Y (normaal enkel LSB) bevat nu de Schiethoek
MOVF REG_Y, 0 ; REG_Y (LSB) to W
MOVWF SchietHoek
; The next code will aim the robot towards the goal
VOOR_MIKKEN
MOVF SchietHoek, 0 ; SchietHoek to W
Call RichtNaarGegevenHoek
Call StilLeggen
RETURN
; *****************************************************************************************************************************************
; ********************************************** Ping ***************************************************************************
; *****************************************************************************************************************************************
; Subroutine that places the distance to the object, seen by the ultrasonic sensor, in Afst_Obj (2 bits / cm)
; Not all comment translated in English
; First we need to send a pulse to the sensor
Ping
BSF STATUS,RP0 ; goto Bank 1
BCF TRISB, 7 ; RB7 signaal to PING))) (input PIC)
BCF STATUS, RP0 ; goto Bank 0
BSF PORTB, 7 ; high op RB7 (signaal naar PING) zetten (begin van de puls)
NOP
NOP
NOP
NOP ; puls moet ongeveer 5 �s duren
BCF PORTB, 7 ; low op RB7 zetten (einde van de puls)
BSF STATUS,RP0 ; goto Bank 1
BSF TRISB, 7 ; RB7 signaal van PING))) (input PIC)
BCF STATUS, RP0 ; goto Bank 0
; Nu wachten tot t_holdoff van de PING))) gedaan is (duurt 750 �s), vanaf dan is RB7 hoog
LOOP1
BTFSS PORTB, 7
GOTO LOOP1 ; t_holdoff is nu gedaan en we beginnen de timing, tot dat RB7 terug laag is
CLRF TMR1L
CLRF TMR1H
LOOP_Poll
BTFSC PORTB,7 ; Als het signaal van de PING terug laag is, uit de loop gaan
GOTO LOOP_Poll
BSF STATUS,RP0 ; goto Bank 1
BCF TRISB, 7 ; RB7 signaal to PING))) (input PIC)
BCF STATUS, RP0 ; goto Bank 0
; Verwerken van de meting
MOVF TMR1L, 0 ;inhoud TMR1L to W
MOVWF TMR1L_END ; 5 te veel!
MOVF TMR1H, 0 ;inhoud TMR1H to W
MOVWF TMR1H_END
MOVF TMR1L_END, 0 ; TMR1L_END to W
MOVWF REG_Z ; TMR1L_END to 8 LSB van REG_Z
MOVF TMR1H_END, 0 ; TMR1H_END to W
MOVWF REG_Z+1 ; TMR1H_END to 8 MSB van REG_Z
MOVLW d'0' ; 0 to W
MOVWF REG_X+1 ; 0 to MSB van REG_X
MOVLW d'5' ; 5 to W
MOVWF REG_X ; 5 to LSB van REG_X
Call M_SUB ; Doe TMR1H_END:TMR1L_END - 5, resultaat zit in Z
; Tussen de inhoud van de timer en de enkele afstand tot het object in bits zit een deling met 29,3. We gaan eerst de inhoud van van de timer
; maal 10 doen, en dan delen door 293 (omdat gewoon delen door 29 niet genoeg nauwkeurigheid geeft als je met microseconden bezig bent).
MOVF REG_Z, 0 ; LSB van Z to W
MOVWF REG_Y ; LSB van Z to LSB van Y
MOVF REG_Z+1, 0 ; MSB van Z to W
MOVWF REG_Y+1 ; MSB van Z to MSB van Y
MOVLW d'0' ; 0 to W
MOVWF REG_X+1 ; 0 to MSB van REG_X
MOVLW d'10' ; 10 to W
MOVWF REG_X ; 10 to LSB van X
Call M_MUL ; Z := X * Y = 10* timer
MOVLW h'25' ; h'25' to W
MOVWF REG_X ; id to REG_X (LSB)
MOVLW h'01' ; h'01' to W
MOVWF REG_X+1 ; id to REG_X (MSB)
Call M_DIV ; Doe 10* TMR1H_END:TMR1L_END / 293, resultaat zit in Y, dit is de afstand tot het object in bits (2 bits / cm) (normaal enkel LSB van Y)
MOVF REG_Y, 0 ; LSB van REG_Y to W
MOVWF Afst_Obj ; LSB van REG_Y to Afst-Obj
RETURN
; *****************************************************************************************************************************************
; ***************************************** IRsInlezen *************************************************************
; *****************************************************************************************************************************************
; Subroutine that refreshes the readings of the IR-sensors and places the results in the bits IR<1>:<3>
; Comment not translated in English
IRsInlezen
BTFSC PORTB, 4 ; IR1 testen
BSF IR, 1 ; IR,1 setten als IR1 hoog is
BTFSS PORTB, 4 ; IR1 testen
BCF IR, 1 ; IR,1 clearen als IR1 laag is
BTFSC PORTB, 5 ; IR2 testen
BSF IR, 2 ; IR,2 setten als IR2 hoog is
BTFSS PORTB, 5 ; IR2 testen
BCF IR, 2 ; IR,2 clearen als IR2 laag is
BTFSC PORTB, 6 ; IR3 testen
BSF IR, 3 ; IR,3 setten als IR3 hoog is
BTFSS PORTB, 6 ; IR3 testen
BCF IR, 3 ; IR,3 clearen als IR3 laag is
RETURN
; *****************************************************************************************************************************************
; ***************************************** SetSpeed ***************************************************************
; *****************************************************************************************************************************************
; Subroutine to use the values in ActionL and Action R to drive the PWMs and thus the motors
SetSpeed
; First the left PWM
BTFSS ActionL, 7 ;
BSF PORTC,3 ;
BTFSC ActionL, 7 ;
BCF PORTC,3 ;
BCF STATUS, C ; To be sure that the following RLF (which rolls through the carry bit) does not make mistakes
RLF ActionL, 0 ; result to W
MOVWF CCPR1L
; Then the right PWM
BTFSS ActionR, 7 ;
BSF PORTC,0 ;
BTFSC ActionR, 7 ;
BCF PORTC,0 ;
BCF STATUS, C ; To be sure that the following RLF (which rolls through the carry bit) does not make mistakes
RLF ActionR, 0 ; result to W
MOVWF CCPR2L
RETURN
; *****************************************************************************************************************************************
; ***************************************** Wait1Second ***************************************************************
; *****************************************************************************************************************************************
; Subroutine (from the internet) of which the execution takes exactly 1 second. It uses WaitNms
Wait1Second
movlw d'250' ;250mS
call WaitNms ;
movlw d'250' ;250mS
call WaitNms ;
movlw d'250' ;250mS
call WaitNms ;
movlw d'250' ;250mS
call WaitNms ;
RETURN
; *****************************************************************************************************************************************
; ***************************************** WaitNms ******************************************************************
; *****************************************************************************************************************************************
; Subroutine (from the internet) of which the execution takes N milliseconds. N is the value in W when this subroutine is called
; Hier begint WaitNms
WaitNms MOVWF Timer_local
LoopNms MOVLW d'248' ; 1 1
CALL Loop1ms ; 995 995
NOP ; 1 1
DECFSZ Timer_local,f ; Dec loop varialble and check 2 1
GOTO LoopNms ;No, keep looping - 2
RETURN ; Yes, Nms done 2
Loop1ms ADDLW d'255' ; Dec W call: 2 addlw 1
BTFSS STATUS, Z ; Zero flag set? 1 2
GOTO Loop1ms ; No, keep looping 2
RETURN ; yes, 1ms done 2
; *****************************************************************************************************************************************
; ***************************************** VooruitEnNaarLinks *******************************************************
; *****************************************************************************************************************************************
; Subroutine that moves the robot ahead and slightly to the left
VooruitEnNaarLinks
MOVLW h'8D'
MOVWF ActionL
MOVLW h'F1'
MOVWF ActionR
Call SetSpeed
MOVLW d'15'
Call WaitNms
Call StilLeggen
RETURN
; *****************************************************************************************************************************************
; ***************************************** VooruitEnNaarRechts *******************************************************
; *****************************************************************************************************************************************
; Subroutine that moves the robot ahead and slightly to the right
VooruitEnNaarRechts
MOVLW h'F1'
MOVWF ActionL
MOVLW h'8D'
MOVWF ActionR
Call SetSpeed
MOVLW d'15'
Call WaitNms
Call StilLeggen
RETURN
; *****************************************************************************************************************************************
; ***************************************** DraaiBeetjeNaarLinks *****************************************************
; *****************************************************************************************************************************************
; Subroutine that turns the robot 5 degrees to the left
DraaiBeetjeNaarLinks
MOVLW H'00'
MOVWF ActionL
MOVLW H'FF'
MOVWF ActionR
Call SetSpeed
MOVLW d'20'
Call WaitNms
Call StilLeggen
MOVLW d'20'
Call WaitNms
RETURN
; *****************************************************************************************************************************************
; ***************************************** DraaiBeetjeNaarRechts *****************************************************
; *****************************************************************************************************************************************
; Subroutine that turns the robot 5 degrees to the right
DraaiBeetjeNaarRechts
MOVLW H'FF'
MOVWF ActionL
MOVLW H'00'
MOVWF ActionR
Call SetSpeed
MOVLW d'20'
Call WaitNms
Call StilLeggen
MOVLW d'20'
Call WaitNms
RETURN
; *****************************************************************************************************************************************
; ***************************************** DraaiNaarLinks *******************************************************
; *****************************************************************************************************************************************
; Subroutine that turns the robot 20 degrees to the left
DraaiNaarLinks
MOVLW H'00'
MOVWF ActionL
MOVLW H'FF'
MOVWF ActionR
Call SetSpeed
MOVLW d'100'
Call WaitNms
Call StilLeggen
RETURN
; *****************************************************************************************************************************************
; ***************************************** DraaiNaarRechts *******************************************************
; *****************************************************************************************************************************************
; Subroutine that turns the robot 20 degrees to the right
DraaiNaarRechts
MOVLW H'FF'
MOVWF ActionL
MOVLW H'00'
MOVWF ActionR
Call SetSpeed
MOVLW d'100'
Call WaitNms
Call StilLeggen
RETURN
; ***************************************************************************************************************************************
; *********************************************** RichtNaarGegevenHoek *****************************************************
; ***************************************************************************************************************************************
; Subroutine that aims the robot to the angle, given in W when this subroutine is called
; Comment not translated to English
RichtNaarGegevenHoek
MOVWF GewensteHoek
Call GRIJS_NAAR_HOEK
MOVF GewensteHoek, 0 ; GewensteHoek to W
MOVWF TEMP_ABS_A ; GewensteHoek to TEMP_ABS_A
MOVF Hoek, 0 ; Hoek to W
MOVWF TEMP_ABS_B ; Hoek to TEMP_ABS_B
Call BerekenAbsoluteWaardeVanVerschil ; hierna zit in W de absolute waarde van het verschil van GewensteHoek en Hoek
SUBWF HoekTolerantie, 0 ; hierna zit in W HoekTolerantie - Abs(GewensteHoek - Hoek)
BTFSC STATUS, C
RETURN ; Als B = C = 1 dan is Abs(Diff)<HoekTolerantie en zitten we dus close genoeg
MOVF Hoek, 0 ; Hoek to W
SUBWF GewensteHoek, 0 ; hierna zit in W GewensteHoek-Hoek
BTFSS STATUS, C
Call DraaiBeetjeNaarRechts
BTFSC STATUS, C
Call DraaiBeetjeNaarLinks
MOVLW d'100'
Call WaitNms
GOTO RichtNaarGegevenHoek
; ***************************************************************************************************************************************
; **************************************** UITL_NAAR_AFSTAND_X_POS_NIEUWE_SHARP *********************************************
; ***************************************************************************************************************************************
; Subroutine that reads the SHARP-IR-sensors and calculates the X-position of the robot
; Not all comment translated to English
UITL_NAAR_AFSTAND_X_POS_NIEUWE_SHARP
; Reading the left SHARP
BCF STATUS, RP0 ; Ga naar bank 0
CLRF ADRESL
CLRF ADRESH ; om vorige conversieresultaten te clearen
MOVLW B'00110001' ; AD module powered on, AN6 inlezen, Fosc/2, not converting
MOVWF ADCON0
BSF ADCON0,GO ; Start AD conv
LOOPAD_Links
BTFSC ADCON0,GO ; Wait until AD conv is done
GOTO LOOPAD_Links
MOVF ADRESH,W
MOVWF Uitl_L ; self-explanatory
; Reading the right SHARP
CLRF ADRESL
CLRF ADRESH ; om vorige conversieresultaten te clearen
MOVLW B'00111001' ; AD module powered on, AN7 inlezen, Fosc/2, not converting
MOVWF ADCON0
BSF ADCON0,GO ; Start AD conv
LOOPAD_Rechts
BTFSC ADCON0,GO ; Wait until AD conv is done
GOTO LOOPAD_Rechts
MOVF ADRESH,W
MOVWF Uitl_R ; self-explanatory
; Every reading can be right of or left of (further then or shorter then) the maximum of the sensor characteristic (see characteristic, around 10 cm = 20 bits)
; We will calculate for both voltages, both possible length --> this gives us 4 possible combinations
; We have to weigh these lengths with the cosine of the angle to get the distance, perpendicular to the wall.
; The perpendicular distances of the right combination will add up to the width of the field
; First: if one of the readings is smaller than 40 bits (see characteristic), then this reading is definitely in the left part of the characteristic; thus, no more combinations have to be made
MOVF Uitl_L,W
SUBLW d'40' ; als er niet geborrowd is, dan is de borrow-bit = 1 en dan is dus links<40 en zitten we dus tegen de muur
MOVF STATUS,W ; status to W
BTFSC STATUS, 0
GOTO LinksTegenMuur ; The left distance is close to the wall (closer than 40 bits)
MOVF Uitl_R,W
SUBLW d'40' ; als er niet geborrowd is, dan is de borrow-bit = 1 en dan is dus links<40 en zitten we dus tegen de muur
MOVF STATUS,W ; status to W
BTFSC STATUS, 0
GOTO RechtsTegenMuur ; The right distance is close to the wall (closer than 40 bits)
; If the preceding code did not work, we will have to make the combinations
; We berekenen nu achtereenvolgens LinksKort (1), LinksVer (2), Rechtskort (3) en RechtsVer (4)
MOVF Hoek, 0 ; Hoek to W
Call HoekNaarEersteKwadrant ; De hoek naar eerste kwadrant herleiden
MOVF Uitl_L, 0 ; Links to W
Call InterpoleerKort
Call LoodrechteAfstandBerekenen
MOVWF LinksKort ; 1
MOVF Uitl_L, 0 ; Links to W
Call InterpoleerVer
Call LoodrechteAfstandBerekenen
MOVWF LinksVer ; 2
MOVF Uitl_R, 0 ; Rechts to W
Call InterpoleerKort
Call LoodrechteAfstandBerekenen
MOVWF RechtsKort ; 3
MOVF Uitl_R, 0 ; Rechts to W
Call InterpoleerVer
Call LoodrechteAfstandBerekenen
MOVWF RechtsVer ; 4
; We gaan nu de combinaties maken: 1+4, 2+3, 2+4. (Niet 1+3 want dat is kort + kort.) Slechts ��n van deze combinaties zal, na weging met de cosinus,
; aanleiding geven tot de juiste som, met name de breedte van het veld.
; Bereken de 3 nuttige sommen. Trek dit van 77 cm * 2 cm/bit = h'9E' af (immers, breedte speelveld min breedte robot) en neem dan de absolute waarde.
; Sla deze telkens op.
MOVF LinksKort, 0 ; LinksKort to W
ADDWF RechtsVer, 0 ; LinksKort + RechtsVer to W
MOVWF TEMP_ABS_A ; LinksKort + RechtsVer to TEMP_ABS_A
MOVLW h'9E'
MOVWF TEMP_ABS_B
Call BerekenAbsoluteWaardeVanVerschil ; Doe ABS(h'9E' - (LinksKort + RechtsVer)), zet in W
MOVWF RES_1_4
MOVF LinksVer, 0 ; LinksVer to W
ADDWF RechtsVer, 0 ; LinksVer + RechtsVer to W
MOVWF TEMP_ABS_A ; LinksVer + RechtsVer to TEMP_ABS_A
MOVLW h'9E'
MOVWF TEMP_ABS_B
Call BerekenAbsoluteWaardeVanVerschil ; Doe h'9E' - (LinksVer + RechtsVer), zet in W
MOVWF RES_2_4 ; LinksVer + RechtsVer to RES_2_4
MOVF LinksVer, 0 ; LinksVer to W
ADDWF RechtsKort, 0 ; LinksVer + RechtsKort to W
MOVWF TEMP_ABS_A ; LinksVer + RechtsKort to TEMP_ABS_A
MOVLW h'9E'
MOVWF TEMP_ABS_B
Call BerekenAbsoluteWaardeVanVerschil ; Doe h'9E' - (LinksVer + RechtsKort), zet in W
MOVWF RES_2_3 ; LinksVer + RechtsKort to RES_2_3
; Welke van de 3 verschillen (verwachte_afstand - berekende_afstand) ligt het dichtst bij 0?
MOVF RES_1_4,W
SUBWF RES_2_4,0 ; aftrekken, resultaat niet nodig (gaat over borrow-bit in status)
BTFSC STATUS, C
Call RES_1_4_Beste ; RES_1_4 is kleiner dan RES_2_4 en dus totnogtoe de beste benadering
BTFSS STATUS, C
Call RES_2_4_Beste ; RES_1_4 is groter dan RES_2_4 dus is RES_2_4 totnogtoe de beste benadering
Call BepaalXPositie
RETURN
; ***************************************************************************************************************************************
; ********************************************* "GRIJS_NAAR_HOEK" *****************************************************
; ***************************************************************************************************************************************
; Subroutine that reads the 3 greyscale values below the robot and calculates the orientation (Hoek) of the robot (angle referred to the vertical) and the y-position
; Not all comment translated to English
GRIJS_NAAR_HOEK
; Reading the 3 greyscale sensors
MOVLW B'00000001'
MOVWF WelkePoort ; AN0 instellen als uit te lezen poort
Call INSTELLEN
Call INLEZEN
MOVWF GrijsA
MOVLW B'00000010'
MOVWF WelkePoort ; AN1 instellen als uit te lezen poort
Call INSTELLEN
Call INLEZEN
MOVWF GrijsB
MOVLW B'00000100'
MOVWF WelkePoort ; AN3 instellen als uit te lezen poort
Call INSTELLEN
Call INLEZEN
MOVWF GrijsC
Call Formule_Z ; To calculate the average of the 3 greyscale readings
Call LOGIC ; This subroutine will calculate Hoek
; Because of non-linearities in the greyscale, the y-position should be adjusted here with a linearization of the greyscale
; However, because of time shortage, we chose for a fixed offset that is good in the middle of the field but bad in the outer areas
MOVLW d'76' ; compensatie voor 1,5 volt ernaast (is zo in het midden van het veld)
SUBWF Gemiddelde, 0 ; W := Gemiddelde - 76
MOVWF Y_Pos
RETURN
; ***************************************************************************************************************************************
; ********************************************* INSTELLEN **********************************************************
; ***************************************************************************************************************************************
; Subroutine that initializes the ports for the reading of the greyscale
; AN0 als WelkePoort,0 = 1
; AN1 als WelkePoort,1 = 1
; AN3 als WelkePoort,2 = 1
INSTELLEN
BTFSC WelkePoort,0
MOVLW B'00000001' ; AD module powered on, channel 0 (AN0) inlezen, Fosc/2
BTFSC WelkePoort,1
MOVLW B'00001001' ; AD module powered on, channel 1 (AN1) inlezen, Fosc/2
BTFSC WelkePoort,2
MOVLW B'00011001' ; AD module powered on, channel 3 (AN3) inlezen, Fosc/2
MOVWF ADCON0 ; Juiste kanaal zal nu ingesteld zijn
RETURN
; ***************************************************************************************************************************************
; ********************************************* INLEZEN **********************************************************
; ***************************************************************************************************************************************
; Subroutine that reads the values of the set port
; resultaat steekt in W
INLEZEN
BSF ADCON0,GO ; Start AD conv
LOOPAD
BTFSC ADCON0,GO ; Wait until AD conv is done
GOTO LOOPAD
MOVF ADRESH,W
RETURN
; ***************************************************************************************************************************************
; ********************************************* LOGIC **********************************************************
; ***************************************************************************************************************************************
; Subroutine (and lots of nested subroutines) that will eventually place the orientation in Hoek
; Comment not translated in English
LOGIC
MOVF GrijsA,W
SUBWF GrijsB,0 ; aftrekken, resultaat niet nodig (gaat over borrow-bit in status)
MOVF STATUS,W ; status to W
MOVWF AGroterDanB ; status to AGroterDanB (borrow-bit is Bit0)
BTFSC AGroterDanB,0
Call ABiggerThanC ; A is kleiner dan B
BTFSS AGroterDanB,0
Call ABiggerThanC_Twee ; A is groter dan B
MOVWF Hoek
RETURN
; "ABiggerThanC"
ABiggerThanC
MOVF GrijsA,W
SUBWF GrijsC,0 ; aftrekken, resultaat niet nodig (gaat over borrow-bit in status)
MOVF STATUS,W ; status to W
MOVWF AGroterDanC ; status to AGroterDanC (borrow-bit is Bit0)
BTFSC AGroterDanC,0
Call BBiggerThanC
BTFSS AGroterDanC,0
Call Form_A_BLC ; A midden en B>C formule!
RETURN
; "BBiggerThanC"
BBiggerThanC
MOVF GrijsB,W
SUBWF GrijsC,0 ; aftrekken, resultaat niet nodig (gaat over borrow-bit in status)
MOVF STATUS,W ; status to W
MOVWF BGroterDanC ; status to BGroterDanC (borrow-bit is Bit0)
BTFSS BGroterDanC,0
Call Form_C_BLA ; C midden en B>A formule!
BTFSC BGroterDanC,0
Call Form_B_CLA ; B midden en C>A formule!
RETURN
; "ABiggerThanC_Twee"
ABiggerThanC_Twee
MOVF GrijsA,W
SUBWF GrijsC,0 ; aftrekken, resultaat niet nodig (gaat over borrow-bit in status)
MOVF STATUS,W ; status to W
MOVWF AGroterDanC ; status to AGroterDanC (borrow-bit is Bit0)
BTFSC AGroterDanC,0
Call Form_A_CLB ; A midden en C>B formule!
BTFSS AGroterDanC,0
Call BBiggerThanC_Twee
RETURN
; "BBiggerThanC_Twee"
BBiggerThanC_Twee
MOVF GrijsB,W
SUBWF GrijsC,0 ; aftrekken, resultaat niet nodig (gaat over borrow-bit in status)
MOVF STATUS,W ; status to W
MOVWF BGroterDanC ; status to BGroterDanC (borrow-bit is Bit0)
BTFSS BGroterDanC,0
Call Form_B_ALC ; B midden en A>C formule!
BTFSC BGroterDanC,0
Call Form_C_ALB ; C midden en A>B formule!
RETURN
; "Formule voor gemiddelde Z"
Formule_Z
CLRW
MOVF GrijsA, 0 ; GrijsA to W
MOVWF REG_X ; GrijsA to X (LSB)
MOVF GrijsB, 0 ; GrijsB to W
MOVWF REG_Z ; GrijsB to Z (LSB)
Call M_ADD ; Z (mog. LSB en MSB) := X + Z = GrijsA + GrijsB
MOVF GrijsC, 0 ; GrijsC to W
MOVWF REG_X ; GrijsC to X (LSB)
Call M_ADD ; Z (mog. LSB en MSB) := X + Z = GrijsC + Z = GrijsC + (GrijsA + GrijsB) = Som(Grijs)
MOVLW d'3' ; 3 to W
MOVWF REG_X ; 3 to X
Call M_DIV ; Y := Z / X = Som(Grijs)/3 = Gemiddelde (normaal enkel LSB), maar to beneden afgerond, en dit moet ZEKER to boven afgerond zijn om rond 0 correct te zijn!
INCF REG_Y, 1 ; Y := Y + 1
MOVF REG_Y, 0 ; Y to W
MOVWF Gemiddelde ; Y to Gemiddelde
; Nu komen 255 bits van de grijswaarde (volledig wit) - 0 bits van de grijswaarde (volledig zwart) overeen met 119*2=238 bits van de lengte (helemaal ;bovenaan) - 0*2=0 bits van de lengte (helemaal onderaan) --> de conversiefactor van grijswaarde naar positie(bits) = 238/255 = 0,93 = 93/100
MOVWF REG_X ; Gemiddelde to X (LSB)
MOVLW d'93' ; 93 to W
MOVWF REG_Y ; 93 to Y (LSB)
Call M_MUL ; Z := X * Y = 93 * Gemiddelde (MSB & LSB)
MOVLW d'100' ; 100 to W
MOVWF REG_X ; 100 to X
Call M_DIV ; Y := Y / X = 93 / 100 * Gemiddelde (normaal enkel LSB)
MOVF REG_Y, 0 ; Y to W
MOVWF Y_Pos ; Y to Y_Pos (opgelet, vanaf hierna zal gemiddelde gebruikt worden voor andere doeleinden!)
RETURN
; Hierna volgen de formules voor de hoekbepaling ifv de gemiddelde grijswaarde en de grijswaarde die daar het verst vanaf ligt
; Formaat van de formule: Teta = +- k * (A/B/C - Z) +- Teta_Ster
; Met k = 8 (gekozen dmv dimensionering) en Teta_Ster functie van de ori�ntatie van de robot
; Alle onderstaande formules eindigen met teta (in bits) in W
; "Formule voor A midden en B>C"
Form_A_BLC
BCF STATUS, C
MOVF Gemiddelde, 0 ; Gemiddelde to W
SUBWF GrijsA, 0 ; W := GrijsA - Gemiddelde
BTFSS STATUS, C
GOTO A_BLC_Neg
GOTO A_BLC_Pos
; "Formule voor C midden en A<B"
Form_C_BLA
BCF STATUS, C
MOVF Gemiddelde, 0 ; zet gemiddelde in W
SUBWF GrijsC, 0 ; W := GrijsC-Gemiddelde
BTFSS STATUS, C
GOTO C_BLA_Neg
GOTO C_BLA_Pos
; "Formule voor C midden en A>B"
Form_C_ALB
BCF STATUS, C
MOVF Gemiddelde, 0 ; zet gemiddelde in W
SUBWF GrijsC, 0 ; W := GrijsC-Gemiddelde
BTFSS STATUS, C
GOTO C_ALB_Neg
GOTO C_ALB_Pos
; "Formule voor B midden en A<C"
Form_B_CLA
BCF STATUS, C
MOVF Gemiddelde, 0 ; zet gemiddelde in W
SUBWF GrijsB, 0 ; W := GrijsB-Gemiddelde
BTFSS STATUS, C
GOTO B_CLA_Neg
GOTO B_CLA_Pos
; "Formule voor A midden en B<C"
Form_A_CLB
BCF STATUS, C
MOVF Gemiddelde, 0 ; zet gemiddelde in W
SUBWF GrijsA, 0 ; W := GrijsA-Gemiddelde
BTFSS STATUS, C
GOTO A_CLB_Neg
GOTO A_CLB_Pos
; "Formule voor B midden en A>C"
Form_B_ALC
BCF STATUS, C
MOVF Gemiddelde, 0 ; zet gemiddelde in W
SUBWF GrijsB, 0 ; W := GrijsB-Gemiddelde
GOTO B_ALC_Neg
GOTO B_ALC_Pos
; "C_BLA_Neg"
; eindigt met teta in W
C_BLA_Neg
SUBLW d'0' ; W := -W
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
ADDLW d'215' ; 5 keer 43 bij tellen
RETURN
; "C_BLA_Pos"
; eindigt met teta in W
C_BLA_Pos
MOVWF TEMP3
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
SUBLW d'0' ; W := -W
ADDLW d'215' ; 5 keer 43 bij tellen
RETURN
; "C_ALB_Neg"
; eindigt met teta in W
C_ALB_Neg
SUBLW d'0' ; W := -W
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
SUBLW d'0' ; W := -W
ADDLW d'86' ; 2 keer 43 bij tellen
RETURN
; "C_ALB_Pos"
; eindigt met teta in W
C_ALB_Pos
MOVWF TEMP3
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
ADDLW d'86' ; 2 keer 43 bij tellen
RETURN
; "B_CLA_Neg"
; eindigt met teta in W
B_CLA_Neg
SUBLW d'0' ; W := -W
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
SUBLW d'0' ; W := -W
ADDLW d'172' ; Vier keer 43 bij tellen
RETURN
; "B_CLA_Pos"
; eindigt met teta in W
B_CLA_Pos
MOVWF TEMP3
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
ADDLW d'172' ; Vier keer 43 bij tellen
RETURN
; "B_ALC_Neg"
; eindigt met teta in W
B_ALC_Neg
SUBLW d'0' ; W := -W
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
ADDLW d'43'
RETURN
; "B_ALC_Pos"
; eindigt met teta in W
B_ALC_Pos
MOVWF TEMP3
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
SUBLW d'0' ; W := -W
ADDLW d'43'
RETURN
; "A_CLB_Neg"
; eindigt met teta in W
A_CLB_Neg
SUBLW d'0' ; W := -W
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
ADDLW d'129' ; Nog 3 keer de term 43 bijtellen
RETURN
; "A_CLB_Pos"
; eindigt met teta in W
A_CLB_Pos
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0
SUBLW d'0' ; W := -W
ADDLW d'129' ; Nog 3 keer de term 43 bijtellen
RETURN
; "A_BLC_Neg"
; eindigt met teta in W
A_BLC_Neg
SUBLW d'0' ; W := -W
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0 ; TEMP3 to W
SUBLW d'0' ; W := -W
RETURN
; "A_BLC_Pos"
; eindigt met teta in W
A_BLC_Pos
BCF STATUS, C
MOVWF TEMP3
RLF TEMP3
BCF STATUS, C
RLF TEMP3
BCF STATUS, C
RLF TEMP3
MOVF TEMP3, 0
RETURN
; ***********************************************************************************************************************************************
; ************************************************* BepaalXPositie *************************************************************
; ***********************************************************************************************************************************************
; Subroutine that uses the perpendicular distances to the right and left walls and calculates the X-position
; Comment not translated to English
BepaalXPositie
MOVLW h'A' ; d'10' = 5 cm (afstand van SHARPs tot artificieel middelpunt van de robot, waar x-positie to refereert)
ADDWF Loodr_Afst_Links, 0 ; W = Loodr_Afst_L + 10 bits
MOVWF X_1 ; X_1 = Loodr_Afst_L + 10 bits
MOVLW h'A' ; d'10' = 5 cm (afstand van SHARPs tot artificieel middelpunt van de robot, waar x-positie to refereert)
ADDWF Loodr_Afst_Rechts, 0 ; W = Loodr_Afst_R + 10 bits
SUBLW d'174' ; 174 bits = 87 cm * 2 bits/cm; bewerking is 174 bits - (Loodr_Afst_R + 10 bits); resultaat in W
MOVWF X_2
BCF STATUS, C
RRF X_1 ; X_1 := X_1/2
BCF STATUS, C
RRF X_2 ; X_2 := X_2/2
MOVF X_1, 0 ; X_1 to W
ADDWF X_2, 0 ; W := X_1 + X_2
MOVWF X_Pos ; GEDAAN!!
RETURN ; return to van waar ganse x positie ding werd opgeroepen
; "RES_1_4_Beste" ; RES_1_4 vergelijken met RES_2_3
RES_1_4_Beste
MOVF RES_1_4,W
SUBWF RES_2_3,0 ; aftrekken, resultaat niet nodig (gaat over borrow-bit in status)
BTFSC STATUS, C
CAll RES_1_4_Beste_End ; RES_1_4 is kleiner dan RES_2_3 en dus uiteindelijk de beste benadering
BTFSS STATUS, C
Call RES_2_3_Beste_End ; RES_1_4 is groter dan RES_2_3 dus is RES_2_3 uiteindelijk de beste benadering
RETURN
; "RES_2_4_Beste"
RES_2_4_Beste
MOVF RES_2_4,W
SUBWF RES_2_3,0 ; aftrekken, resultaat niet nodig (gaat over borrow-bit in status)
BTFSC STATUS, C
CAll RES_2_4_Beste_End ; RES_2_4 is kleiner dan RES_2_3 en dus uiteindelijk de beste benadering
BTFSS STATUS, C
Call RES_2_3_Beste_End ; RES_2_4 is groter dan RES_2_3 dus is RES_2_3 uiteindelijk de beste benadering
RETURN
; "RES_1_4_Beste_End"
RES_1_4_Beste_End
MOVF LinksKort, 0 ; 1 (LinksKort) to W
MOVWF Loodr_Afst_Links
MOVF RechtsVer, 0 ; 4 (RechtsVer) to W
MOVWF Loodr_Afst_Rechts
RETURN
; "RES_2_3_Beste_End"
RES_2_3_Beste_End
MOVF LinksVer, 0 ; 2 (LinksVer) to W
MOVWF Loodr_Afst_Links
MOVF RechtsKort, 0 ; 3 (RechtsKort) to W
MOVWF Loodr_Afst_Rechts
RETURN
; "RES_2_4_Beste_End"
RES_2_4_Beste_End
MOVF LinksVer, 0 ; 2 (LinksVer) to W
MOVWF Loodr_Afst_Links
MOVF RechtsVer, 0 ; 4 (RechtsVer) to W
MOVWF Loodr_Afst_Rechts
RETURN
; ***********************************************************************************************************************************************
; ************************************************* RechtsTegenMuur *************************************************************
; ***********************************************************************************************************************************************
; Subroutine that takes the values in TEMP_ABS_A an and TEMP_ABS_B and calculates the absolute value of the difference between them. Result in W
BerekenAbsoluteWaardeVanVerschil
MOVF TEMP_ABS_A, 0 ; Move TEMP_ABS_A to W
SUBWF TEMP_ABS_B, 0 ; TEMP_ABS_B - TEMP_ABS_A to W
BTFSS STATUS, C ; skippen als Borrow = 1 maw als er niet geborrowd is maw als TEMP_ABS_B>TEMP_ABS_A
SUBLW h'00' ; W bevat nu 00 - W maw het inverse van voorgaande negatieve verschil
RETURN
; ***********************************************************************************************************************************************
; ************************************************* LinksTegenMuur *************************************************************
; ***********************************************************************************************************************************************
; Subroutine that calculates the perpendicular distances (left and right) when the robot is known to be very close to the left wall
; Comment not translated to English
LinksTegenMuur
MOVF Uitl_L, 0 ; Links to W
Call InterpoleerKort ; Links is immers zeker kort
MOVWF Afstand_Links ; W is de afstand in bits (2 bits / cm) tot de LINKER muur, NIET LOODRECHT
Call HoekNaarEersteKwadrant
Call LoodrechteAfstandBerekenen
MOVWF Loodr_Afst_Links ; loodrechte afstand to juiste register
MOVF Uitl_R, 0 ; Rechts to W
Call InterpoleerVer ; We weten immers dat rechts ver is, want links was kort
MOVWF Afstand_Rechts ; W is de afstand in bits (2 bits / cm) tot de RECHTER muur, NIET LOODRECHT
Call HoekNaarEersteKwadrant
Call LoodrechteAfstandBerekenen
MOVWF Loodr_Afst_Rechts ; loodrechte afstand to juiste register
GOTO BepaalXPositie
; ***********************************************************************************************************************************************
; ************************************************* RechtsTegenMuur *************************************************************
; ***********************************************************************************************************************************************
; Subroutine that calculates the perpendicular distances (left and right) when the robot is known to be very close to the right wall
; Comment not translated to English
RechtsTegenMuur
MOVF Uitl_R, 0 ; Rechts to W
Call InterpoleerKort ; Rechts is immers zeker kort
MOVWF Afstand_Rechts ; W is de afstand in bits (2 bits / cm) tot de RECHTER muur, NIET LOODRECHT
Call HoekNaarEersteKwadrant
Call LoodrechteAfstandBerekenen
MOVWF Loodr_Afst_Rechts ; loodrechte afstand naar juiste register
MOVF Uitl_L, 0 ; Links to W
Call InterpoleerVer ; We weten immers dat links ver is, want rechts was kort
MOVWF Afstand_Rechts ; W is de afstand in bits (2 bits / cm) tot de LINKER muur, NIET LOODRECHT
Call HoekNaarEersteKwadrant
Call LoodrechteAfstandBerekenen
MOVWF Loodr_Afst_Links ; loodrechte afstand to juiste register
GOTO BepaalXPositie
; ***********************************************************************************************************************************************
; ******************************************** HoekNaarEersteKwadrant *********************************************************
; ***********************************************************************************************************************************************
; Subroutine that complements Hoek until it is in the first quadrant
; 0 graden = b'00000000' en 45 graden = b'00111111'
; 315 graden = b'11000000' en de laatste waarde = b'11111111' --> bit 7 steeds 1
HoekNaarEersteKwadrant
BTFSC Hoek, 7 ; Als bit 7 set is, moeten we complementeren
Call ComplementeerHoek
; Als bit 7 clear is, is er geen probleem en doen we gewoon niks met de hoek
MOVWF HoekEersteKwadr ; Al dan niet gecomplementeerde hoek in HoekEersteKwadr zetten
RETURN
; "Complementeer_Hoek"
ComplementeerHoek
MOVF Hoek, 0 ; Zet Hoek in W
SUBLW h'00' ; W bevat nu 00 - W maw het inverse --> de gecomplementeerde hoek!
RETURN
; ***********************************************************************************************************************************************
; ******************************************** LoodrechteAfstandBerekenen *****************************************************
; ***********************************************************************************************************************************************
; Subroutine that takes the non-perpendicular distance (in W) and scales it with the cosine of the orientation angle of the robot (in Hoek)
; Result in W
; Comment not translated in English
LoodrechteAfstandBerekenen
MOVWF SchuineZijde ; Inhoud van W naar tijdelijke parameter
MOVF HoekEersteKwadr, 0 ; gecomplementeerde hoek to W
MOVWF REG_X ; in 8 LSB van X zetten
MOVF SchuineZijde, 0 ; schuine zijde to W
MOVWF REG_Y ; in 8 LSB van Y zetten
Call M_MUL ; na deze call staat het product van HoekEersteKwadr en schuine zijde in REG_Z (LSB) en REG_Z+1 (MSB)
MOVLW d'27' ; 108/4 to W
MOVWF REG_X ; 108/4 to X zetten
Call M_DIV ; na deze call staat het quotient van Hoek*Afst/27 in REG_Y , maar we moeten hebben Hoek*Afst/27 en dus moet er nog eens door 4 ; gedeeld worden --> RRF (RRF (REG_Y))
MOVLW REG_Y ; adres van REG_Y to W
Call M_ROR ; REG_Y / 2
MOVLW REG_Y ; adres van REG_Y to W
Call M_ROR ; REG_Y / 4
MOVF REG_Y, 0 ; 8 LSB van Y to W
MOVWF REG_X ; 8 LSB van Y zijn nu ook 8 LSB van X
MOVF REG_Y+1, 0 ; 8 MSB van Y to W
MOVWF REG_X+1 ; 8 MSB van Y zijn nu ook 8 MSB van X
; (want aftrekken is Z-X en wat moet afgetrokken worden, stond in Y, dus moest Y naar X gezet worden)
MOVF SchuineZijde, 0 ; schuine zijde to W
MOVWF REG_Z ; schuine zijde to Z
Call M_SUB ; na deze call staat de LOODRECHTE AFSTAND TOT DE LINKERMUUR in Z (normaal enkel 8 LSB)
MOVF REG_Z, 0 ; loodrechte afstand to W
RETURN
; ***********************************************************************************************************************************************
; ************************************************* InterpoleerKort *************************************************************
; ***********************************************************************************************************************************************
; Subroutine that interpolates over the "short" branch of the SHARP-characteristic, takes the reading in W
; Result in W
; Comment not translated in English
InterpoleerKort
MOVWF REG_Z ; uitlezing to LSB van Z (is normaal begrensd op FF)
MOVLW d'0' ; 0 to W
MOVWF REG_Z+1 ; 0 to MSB van Z
MOVLW d'13' ; 13 to W
MOVWF REG_X ; 13 to LSB van X
MOVLW d'0' ; 0 to W
MOVWF REG_X+1 ; 0 to MSB van X
Call M_DIV ; Y := Z / X = uitlezing / 13 = afstand tot muur in bits
MOVF REG_Y, 0 ; zet LSB van Y in W
RETURN
; ***********************************************************************************************************************************************
; ************************************************* InterpoleerVer *************************************************************
; ***********************************************************************************************************************************************
; Subroutine that interpolates over the "far" branch of the SHARP-characteristic, takes the reading in W
; Result in W
; Comment not translated in English
InterpoleerVer
MOVWF TEMP ; uitlezing to TEMP
MOVLW d'10' ; 10 to W
SUBWF TEMP, 0 ; W := TEMP - 10 = uitlezing - 10 < h'FF'
MOVWF REG_X ; bovenstaande uitdrukking to LSB van X
MOVLW d'0' ; 0 to W
MOVWF REG_X+1 ; 0 to MSB van X
MOVLW h'27' ; d'9984' to MSB van Z
MOVWF REG_Z+1
MOVLW h'10' ; d'16' to LSB van Z
MOVWF REG_Z
Call M_DIV ; Y := Z / X = 10000 / (V - 10)
MOVF REG_Y+1, 0 ; MSB van Y to W
MOVWF REG_Z+1 ; MSB van Y to Z
MOVF REG_Y, 0 ; LSB van Y to W
MOVWF REG_Z ; LSB van Y to Z
MOVLW d'0' ; 0 to W
MOVWF REG_X+1 ; 0 to MSB van X
MOVLW d'3' ; 3 to W
MOVWF REG_X ; 0 to LSB van X
Call M_SUB ; Z := Z - X = (10000 / (V - 10)) - 3
BCF STATUS, C ; voor de zekerheid
MOVLW REG_Z
Call M_ROR ; Z := Z / 2 = afstand in bits
MOVF REG_Z, 0 ; zet TEMP in W
RETURN
; ***********************************************************************************************************************************************
; ************************************************* StilLeggen *************************************************************
; ***********************************************************************************************************************************************
; Subroutine that puts the robot to a standstill
StilLeggen
MOVLW h'80'
MOVWF ActionL
MOVLW h'80'
MOVWF ActionR
Call SetSpeed
RETURN
; ***********************************************************************************************************************************************
; ******************************************************** ISR *****************************************************************
; ***********************************************************************************************************************************************
ISR
NOP
RETFIE
; ***********************************************************************************************************************************************
; ************************************************* Math-library *************************************************************
; ***********************************************************************************************************************************************
; From the internet: http://avtanski.net/projects/math
; Very helpful!
M_CLR ; clear a register (adres in W)
movwf FSR
movlw PRECISION
movwf REG_COUNTER
M_CLR_loop
clrf INDF
incf FSR,f
decf REG_COUNTER,f
btfss STATUS,Z
goto M_CLR_loop
return
M_INC ; increment a register (adres in W)
movwf FSR
movlw PRECISION
movwf REG_COUNTER
M_INC_loop
incf INDF,f
btfss STATUS,Z
return
incf FSR,f
decf REG_COUNTER,f
btfss STATUS,Z
goto M_INC_loop
return
M_DEC ; decrement a register (adres in W)
movwf FSR
movlw PRECISION
movwf REG_COUNTER
M_DEC_loop
decf INDF,f
movlw 0xFF
subwf INDF,w
btfss STATUS,Z
return
incf FSR,f
decf REG_COUNTER,f
btfss STATUS,Z
goto M_DEC_loop
return
M_ROL ; rotate a register to the left (adres in W)
movwf FSR
M_STOR_STATUS REG_STATUS
clrf REG_COUNTER
M_ROL_loop
M_RETR_STATUS REG_STATUS
rlf INDF,f
M_STOR_STATUS REG_STATUS
incf FSR,f
incf REG_COUNTER,f
movlw PRECISION
subwf REG_COUNTER,w
btfss STATUS,Z
goto M_ROL_loop
return
M_ROR ; rotates a register to the right (adres in W)
movwf FSR
movlw PRECISION-1
addwf FSR,f
M_STOR_STATUS REG_STATUS
clrf REG_COUNTER
M_ROR_loop
M_RETR_STATUS REG_STATUS
rrf INDF,f
M_STOR_STATUS REG_STATUS
decf FSR,f
incf REG_COUNTER,f
movlw PRECISION
subwf REG_COUNTER,w
btfss STATUS,Z
goto M_ROR_loop
return
M_CMP ; Z <=> X -> STATUS(C,Z)
; STATUS,C set if Z => X;
; STATUS,Z set if Z == X
clrf REG_COUNTER
M_CMP_loop
movf REG_COUNTER,w
sublw REG_Z+PRECISION-1
movwf FSR
movf INDF,w
movwf REG_T1
movf REG_COUNTER,w
sublw REG_X+PRECISION-1
movwf FSR
movf INDF,w
subwf REG_T1,f
btfss STATUS,Z
return
incf REG_COUNTER,f
movlw PRECISION
subwf REG_COUNTER,w
btfss STATUS,Z
goto M_CMP_loop
return
M_ADD ; Z + X -> Z
bcf STATUS,C
clrf REG_STATUS
clrf REG_COUNTER
M_ADD_loop
clrf REG_T1
btfsc REG_STATUS,C
incf REG_T1,f
clrf REG_STATUS
movlw REG_X
addwf REG_COUNTER,w
movwf FSR
movf INDF,w
addwf REG_T1,f
btfsc STATUS,C
bsf REG_STATUS,C
movlw REG_Z
addwf REG_COUNTER,w
movwf FSR
movf INDF,w
addwf REG_T1,f
btfsc STATUS,C
bsf REG_STATUS,C
movf REG_T1,w
movwf INDF
incf REG_COUNTER,f
movlw PRECISION
subwf REG_COUNTER,w
btfss STATUS,Z
goto M_ADD_loop
return
M_SUB ; Z - X -> Z
clrf REG_COUNTER
bsf REG_STATUS,C
M_SUB_loop
bsf REG_T2,C
movlw REG_Z
addwf REG_COUNTER,w
movwf FSR
movf INDF,w
movwf REG_T1
movlw REG_X
addwf REG_COUNTER,w
movwf FSR
movf INDF,w
subwf REG_T1,f
btfss STATUS,C
bcf REG_T2,C
btfsc REG_STATUS,C
goto M_SUB_no_carry
movlw 0x01
subwf REG_T1,f
btfss STATUS,C
bcf REG_T2,C
M_SUB_no_carry
movlw REG_Z
addwf REG_COUNTER,w
movwf FSR
movf REG_T1,w
movwf INDF
bsf REG_STATUS,C
btfss REG_T2,C
bcf REG_STATUS,C
incf REG_COUNTER,f
movlw PRECISION
subwf REG_COUNTER,w
btfss STATUS,Z
goto M_SUB_loop
btfss REG_STATUS,C
bcf STATUS,C
return
M_MUL ; X * Y -> Z
movlw REG_Z
call M_CLR
movlw PRECISION*8+1
movwf REG_ROT_COUNTER
M_MUL_loop
decf REG_ROT_COUNTER,f
btfsc STATUS,Z
return
btfsc REG_Y,0
call M_ADD
bcf STATUS,C
movlw REG_Y
call M_ROR
bcf STATUS,C
movlw REG_X
call M_ROL
goto M_MUL_loop
M_DIV ; Z / X -> Y; remainder -> Z
movlw REG_Y
call M_CLR
movlw PRECISION*8
movwf REG_ROT_COUNTER
M_DIV_rot_loop
btfsc REG_X+PRECISION-1,7
goto M_DIV_loop
movlw REG_X
bcf STATUS,C
call M_ROL
decf REG_ROT_COUNTER,f
btfss STATUS,Z
goto M_DIV_rot_loop
bsf STATUS,Z
return
M_DIV_loop
call M_CMP
M_STOR_STATUS REG_T2
movlw REG_Y
call M_ROL
M_RETR_STATUS REG_T2
btfsc STATUS,C
call M_SUB
bcf STATUS,Z
bcf STATUS,C
movlw REG_X
call M_ROR
incf REG_ROT_COUNTER,f
movlw PRECISION*8+1
subwf REG_ROT_COUNTER,w
btfss STATUS,Z
goto M_DIV_loop
return
END