; *********************************************************************************************************************************************** ; 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) 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 AB" 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 AC" 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