title "McGraw-Hill Robot Code" #define _version "1.52" ; ; Version 1.52 - "Debounce" incoming Remote Control Commands from repeated commands ; - Direct Commands (Stop/Forward/Reverse/Turn Right/Turn Left) not Debounced ; - Behavior Starts, BS2 Buttons, and PWM Speed Changes Debounced to 255 msecs ; ; Version 1.51 - Turn Off Collision Detection after Direct Command ; - "Flag" bit is "IROff" ; - Speed is 100% ; ; Version 1.50 - Keypad remapped for Blue/Red Remote Controls ; - RB5 used for Red/Blue Selection (Connected to Swtich) ; - RB5 Pull Up for "Blue" (original) ; - Delete "LEDOn"/"LEDOff" Commands ; ; Start of SumoBot Development Above ; ; ; End of TEBYORK Development Below ; ; Version 0.92 - 1st Release Version ; ; Version 0.91 - Eliminate Full Speed Update on Collision in Behavior1 ; ; Version 0.90 - Scale the Motor PWM to better Values: ; - Full 52/52 (4 Notches) - 100% ; - 3 Notches 45/52 - 86% ; - 2 Notches 39/52 - 75% ; - 1 Notch 33/52 - 63% ; - Stopped 0/52 - 0% ; - Start at 86% to save the battery ; ; Version 0.83 - BS2 Routines updated for faster serial communications of the BS2SX ; ; Version 0.82 - Remote Control Button Operation ; ; Version 0.81 - Fix Problems with BS2 "RobotWhisker" Return ; ; Version 0.80 - Start to Handle the BS2 Commands ; - If BS2 Command is Received, then LED is not changed by application ; ; Version 0.77 - Look at BS2Read: Toggle the LED when a Valid Byte is Received ; - In PWMExecute, Turn OFF Motors before BS2 Data is Read ; - Eliminated the "BS2Response" from the Different Operations ; - Turn off Collision Detection for IR Movement ; ; Version 0.76 - Tuning Basic Operations of the Robot: ; - Behavior1: After Collision, if robot was about to turn, ; continue turning in the direction that the robot was ; turning in response to the collision (Before Changing, the ; section of Behavior1 returned to was found to be wrong) ; - Problem Persisted, Changed the Return Address to ; Forward movement ; - Reduce the time of the Collision turn by 25% ; - Behavior2/3: Reduce time for turn/movement to 80 msecs ; - Behavior4: Reduce time for turn/movement to 80 msecs ; - Direct Control of Forward/Reverse/Turn Left/Turn Right: ; After movement, put the robot in "wait/stop" mode ; - Reduce Active Time to 140 msecs ; ; Version 0.75 - Deleted "InitDlay" and used "PWMExecute" with "CollideIgnore" Set ; and "WheelValue" equal to WheelStop ; ; Version 0.74 - Move "PWMExecute" into the First Page to See if Size Efficiencies ; can be gained ; - Collision LED Turned Off at the start of all Different Operations ; Except for "Stop" ; - Made Sure "PWMExecute" Collision/Operation Change Flags (Carry/Zero) ; used consistently ; ; Version 0.73 - Based "InitDlay" on "Stop" and "PWMExecute" Code ; - Update ReadIRBit to be more tolerant of IR Signals received ; during PWM Output ; - Reordered "ReadIRBit" to Take Advantage of Paging ; - Updated "Behavior4" to work with Changes to Whisker Operation ; ; Version 0.72 - Change the "PWMExecute" Routine to Match the "Stop" Test Code ; ; Version 0.71 - Changed Stop Code to Check over a 20 msec Interval ; - If Count >= 20 then Turn on LED ; - Change IR Read to Skip Over the Start Low and Sync High before ; turning on LED ; ; Version 0.70 - Convert Application to use I/R Receivers instead of physical ; "Whiskers" for collision detection ; - I/R LED connected to Pin RC5 (where I/R Receiver was in Previous Versions) ; - Toggled at 40 KHz for 8 Full Cycles ; - I/R Receivers connected to Whisker Pins (RB0 - Left/RB1 - Right) ; ; Version 0.68 - Get Direct Control Working Again ; - If IRCommand == Repeat then Reset Time to 200 msec Delay ; ; Version 0.67 - Look at Behavior4 and Run at Full Speed ; ; Version 0.66 - Look at Unexpected IR Receive During Operation/Actually 9V Battery ; Power Loss ; ; Version 0.65 - Put in Full On/Full Off Code ; - Increase Off Time from 200 to 400 msec to make it more noticable ; ; Version 0.64 - Change Operation of PWMExecute to allow 0 to 100% ; - Run PWM at 20 KHz with 5 Speed Levels (0 - 4) ; ; Version 0.63 - Put in "Debug3" which is the ability to disable collision checks ; - Put "InitDlay" delay variables in Bank1 ; - Reordered some code to make sure initial starts do not screw up operation ; - Base InitDlay/PWMExecute on 20 msec Loop ; ; Version 0.62 - Looking for operational problems with the Robot ; - Change InitDlay to work at 100 msecs intervals so it can be ; used as a Stop Delay Routine ; - During Initial PowerUp, Read I/R Inputs (Part of InitDlay) ; ; Version 0.61 - Fixed LED On Problems ; ; Version 0.60 - Add Behavior4 ; - Right Whisker should be touching a wall at all times ; - If Right Whisker switch closed, turn left ; - If Right Whisker switch open, turn Right ; - Ignore Left Whisker ; - Invert the Whisker Sensors ; - Light the LED while I/R Data is being received ; - Make Sure the LED is lit on B1 Collisions ; ; Version 0.50 - Add Behavior3 (Photophobe) using Photorvore code ; - clear 2 LSBs from CDS Cell count to filter out noise ; - In Behavior2 and Behavior3, if Return is equal, just go forward ; ; Version 0.41 - Transposed CDSR and LEDBit Due to Different Operation of OSC1 Pin ; ; Version 0.40 - Start working through the ADC Measurements ; - Put in ADC Read Subroutines ; ; Version 0.30 - Put in Direct Robot Control Code ; - Look at Repeat Codes ; ; Version 0.23 - Correct I/R Commands in Code ; - Responding to I/R Speed Commands/B1/Stop ; - Cleaned up Behavior1 Error Handling ; ; Version 0.22 - Put in New Glitch Filter Software ; - Put in I/R Command Handler Code ; - Note I/R Controller Supercedes the BS2 ; - I/R Controlled Movement will have to monitor button operation ; and watch for "Repeat" character ; - Other buttons will ignore the "Repeat" character ; - The BS2 will be able to monitor what the Robot is ; doing, but will only be able to poll the Buttons. ; - When the Buttons are read, the storage location ; is erased until the next I/R Button is ; Received ; ; Version 0.21 - Change Return from IR/BS2/Collisions ; - Put in BS2 Command Interface ; ; Version 0.20 - Put in I/R Interface ; - Output for Key Mapping ; ; Version 0.10 - Start of Application Development ; - Define PWM Loop, Start I/R Interface ; ; PWM Operation: PWM is controlled by "PWMOnValue" variable. ; This variable is in the range of 0 to 4, Giving the PWM a Range of ; 0% to 100%. ; ; I/R Commands: ; 1 - Forward ; 2 - Reverse ; 3 - Left Turn (use both sets of wheels) ; 4 - Right turn (use both sets of wheels) ; 5 - Stop (Including Behaviors) ; 6 - Behavior 1 (Random Motion) ; 7 - Behavior 2 (Photovore) ; 8 - Behavior 3 (Photophobe) ; 9 - Behavior 4 (Maze Follower) ; 10 - PWM Speed Up ; 11 - PWM Speed Down ; 12 - BS2 Command 1 ; 13 - BS2 Command 2 ; 14 - BS2 Command 3 ; ; Buttons and Button Functions are ; ; +-------------+-------------+-------------+ ; | | | | ; | Behavior1 | Forward | Behavior2 | ; | | | | ; +-------------+-------------+-------------+ ; | | | | ; | Turn Left | Stop | Turn Right | ; | | | | ; +-------------+-------------+-------------+ ; | | | | ; | Behavior3 | Reverse | Behavior4 | ; | | | | ; +-------------+-------------+-------------+ ; | | | | ; | Button1 | Button2 | Button3 | ; | | | | ; +-------------+------+------+-------------+ ; | | | ; | Slow Down | Speed Up | ; | | | ; +--------------------+--------------------+ ; ; Blue Codes Behavior1_But EQU 0x068 ; Basic Random Operation Behavior2_But EQU 0x028 ; Photovore Behavior3_But EQU 0x050 ; Photophobe Behavior4_But EQU 0x010 ; Wall Hugging/Maze Solution SlowDown_But EQU 0x080 ; Lower the PWM Speed SpeedUp_But EQU 0x000 ; Increase the PWM Speed Stop_But EQU 0x0B0 ; Stop Robot Regardless of Command Executing Button1_But EQU 0x060 ; Button Value Returned to BS2 Button2_But EQU 0x0A0 ; Button Value Returned to BS2 Button3_But EQU 0x020 ; Button Value Returned to BS2 Forward_But EQU 0x0A8 ; Move the Robot Forward Reverse_But EQU 0x090 ; Move the Robot Backwards TurnLeft_But EQU 0x070 ; Turn the Robot to the Left TurnRight_But EQU 0x030 ; Turn the Robot to the Right ; ; Red Codes - With bit "7" of IR Data (my bit 0) Set Red_Behavior1_But EQU 0x069 ; Basic Random Operation Red_Behavior2_But EQU 0x029 ; Photovore Red_Behavior3_But EQU 0x051 ; Photophobe Red_Behavior4_But EQU 0x011 ; Wall Hugging/Maze Solution Red_SlowDown_But EQU 0x081 ; Lower the PWM Speed Red_SpeedUp_But EQU 0x001 ; Increase the PWM Speed Red_Stop_But EQU 0x0B1 ; Stop Robot Regardless of Command Executing Red_Button1_But EQU 0x061 ; Button Value Returned to BS2 Red_Button2_But EQU 0x0A1 ; Button Value Returned to BS2 Red_Button3_But EQU 0x021 ; Button Value Returned to BS2 Red_Forward_But EQU 0x0A9 ; Move the Robot Forward Red_Reverse_But EQU 0x091 ; Move the Robot Backwards Red_TurnLeft_But EQU 0x071 ; Turn the Robot to the Left Red_TurnRight_But EQU 0x031 ; Turn the Robot to the Right ; ; ; BS2 Commands: ; 0 - Stop ; 1 - Execute Behavior 1 ; 2 - Execute Behavior 2 ; 3 - Execute Behavior 3 ; 4 - Execute Behavior 4 ; 5 - Forward ; 6 - Reverse ; 7 - Turn Left ; 8 - Turn Right ; 9 - nop (formerly LED On) ; 10 - nop (formerly LED Off) ; 11 - Set Motor Speed as Stop ; 12 - Set Motor Speed as First "Notch" ; 13 - Set Motor Speed as Second "Notch" ; 14 - Set Motor Speed as Third "Notch" ; 15 - Set Motor Speed as Full ; 16 - Return Current PWM Value ; 17 - Return Current Operational State (reply with CurrentState) ; 18 - Return Whiskers (reply follows) ; 19 - Return Left CDS (reply follows) ; 20 - Return Right CDS (reply follows) ; 21 - Return Waiting Button (reply follows, 0 (No Buttons), 1, 2 or 3) ; ; ; ; ; ; ; Written by: myke predko - myke@passport.ca ; ; Copyright (C) 2002 - McGraw-Hill ; ; This application is provided for single user evaluation only. Redistribution of the application ; is only permitted with written permission of McGraw-Hill. ; ; ; ; ; ; ; Hardware Notes: ; Internal Oscillator (4 MHz) and Reset used for the application ; RB0 - Negative Active Left "Whisker" (IR Detector) #define LeftIR PORTB, 0 ; RB1 - Negative Active Right "Whisker" (IR Detector) #define RightIR PORTB, 1 ; RB2 - Right CDS Cell ADC ; RightCDS EQU B'011011' ; #### - Original TEBYORK Value RightCDS EQU B'111011' #define RightCDSBit PORTB, 2 ; RB3 - BS2 Serial Clock (Input ONLY) #define BS2Clock PORTB, 3 ; RB4 - BS2 Serial Data (Bi-Directional) #define BS2Data PORTB, 4 ; SERIALOut EQU B'001111' ; #### - Original TEBYORK Value SERIALOut EQU B'101111' ; RB5 - Negative Active LED ; LEDTris EQU B'011111' ; #### - Original TEBYORK Value LEDTris EQU B'111111' ; #define LEDBit PORTB, 5 ; #### - Code taken from original TEBYORK ; RC0 - Left Reverse Motor Control ; RC1 - Left Forward Motor Control ; RC2 - Right Reverse Motor Control ; RC3 - Right Forward Motor Control PORTCTris EQU B'010000' Forward EQU B'111010' Reverse EQU B'110101' Left EQU B'111001' Right EQU B'110110' WheelStop EQU B'110000' ; RC4 - Left CDS Cell ADC LeftCDS EQU B'000000' #define LeftCDSBit PORTC, 4 ; RC5 - I/R Remote Control LED Output (Output ONLY) #define IRLEDBit PORTC, 5 list R=DEC include "p16C505.inc" ; Constants TwoSecs EQU 100 OneSec EQU 50 HalfSec EQU 25 ThreeQuarterSec EQU 37 ; Register Usage CBLOCK 0x008 ; Start of GP Registers CurrentState ; Current Operational State ; = 0x000 - Stopped, waiting for Command ; = 0x001 - Behavior 1 (Random Motion) ; = 0x002 - Behavior 2 (Photovore) ; = 0x003 - Behavior 3 (Photophobe) ; = 0x004 - Behavior 4 (Maze Follower) ; = 0x005 - I/R Forward Command ; = 0x006 - I/R Reverse Command ; = 0x007 - I/R Left Command ; = 0x008 - I/R Right Command Flag ; Status Bits of Operation i, j, k LeftCollideCount, RightCollideCount ; Collision Counters PWMOnValue ; PWM Operation Counter PWMCount:4 WheelValue ; Wheel Movement Value IRCommand ; Last Byte Read by IR ReturnIndex ; Calling Program Return Index Dlay:2 ; Counters for Delaying Operations Neg1 ; Constant Value for Decrementing OneFirst, OneSecond ; Ranges for Bit Values ZeroFirst, ZeroSecond ContFirst, ContSecond Bank0VariableEnd ENDC if (Bank0VariableEnd > 0x020) error "Too Many Variables In Bank 0 Used in Application endif CBLOCK 0x030 ; Start of GP Registers LastButton ; Last Button Pressed by I/R IRCommandCount, IRCommandDlay ; Count since the last Variables used for IR "debounce" ; NOTE: IRCommandCount must be one address BEFORE IRCommandDlay Bank1VariableEnd ENDC if (Bank1VariableEnd > 0x040) error "Too Many Variables In Bank 1 Used in Application endif CBLOCK 0x050 ; Start of GP Registers BS2Byte ; Byte Returned from BS2 Bank2VariableEnd ENDC if (Bank2VariableEnd > 0x060) error "Too Many Variables In Bank 2 Used in Application endif CBLOCK 0x070 ; Start of GP Registers IRTable:4 ; Save 32 Bits of I/R Data Bank3VariableEnd ENDC if (Bank3VariableEnd > 0x080) error "Too Many Variables In Bank 2 Used in Application endif ; Flag Definitions #define LeftCollide Flag, 0 ; Bit Set when Collision on Left Whisker #define RightCollide Flag, 1 ; Bit Set when Collision on Right Whisker #define CollideIgnore Flag, 2 ; Flag Set for PWMExecute if Collisions are to be Ignored #define BS2Active Flag, 3 ; Bit Set After BS2 Command is Received - Only Actual ; Effect is that LED cannot be Change except by BS2 #define SaveLeftCollide Flag, 4 ; Bit Set when Collision on Left Whisker #define SaveRightCollide Flag, 5 ; Bit Set when Collision on Right Whisker #define IROff Flag, 6 ; IR Disable Flag - Set to Stop IR Detection PAGE ; Macros callm Macro Label ; Long Subroutine Call if ((Label & 0x0200) == ($ & 0x0200)) messg "Far Call Not Required" endif if ((Label & 0x0200) == 0) bcf STATUS, PA0 else bsf STATUS, PA0 endif call (Label & 0x01FF) | ($ & 0x0200) if (($ & 0x0200) == 0) bcf STATUS, PA0 else bsf STATUS, PA0 endif endm PAGE __CONFIG _MCLRE_OFF & _CP_OFF & _WDT_OFF & _IntRC_OSC_RB4EN ; Mainline Code org 0 movwf OSCCAL ; Save the Oscillator Calibration Value Initialize movlw B'10011111' ; Allow RC5 to be a Digital Input option movlw B'111111' ; Initialize the I/O Ports movwf PORTB movlw LEDTris tris PORTB movlw WheelStop movwf PORTC clrf FSR movlw 0x0FF ; Setup Constants for Operation movwf Neg1 movlw 255 - 50 ; Setup the I/R Bit Read Limits movwf OneFirst movlw 50 - 27 + 1 movwf OneSecond movlw 255 - 135 movwf ZeroFirst movlw 135 - 93 + 1 movwf ZeroSecond movlw 255 - 165 ; Do the Continuation Values as well movwf ContFirst movlw 165 - 150 + 1 movwf ContSecond clrf CurrentState ; If BS2 Polls State, it will return "Stop" movlw IRCommandCount ; Initialize IRCommandCount to accept an IR Command Immediately movwf FSR movlw 0x0FF movwf INDF movlw LastButton ; Make Sure Last Button is Cleared movwf FSR clrf INDF clrf FSR movlw 4 ; Assume Initial Operation is FULL Speed movwf PWMOnValue movlw WheelStop ; Use PWMExecute with No Wheel Motion movwf WheelValue bsf CollideIgnore ; As Part of this, Ignore Collisions bsf IROff ; Turn off IR Detection movlw 24 call PWMExecute ; Delay 0.5 Second with LED Off btfss STATUS, Z goto Loop ; If NO Carry, then Process BS2 Command ; btfss BS2Active ; #### - Removed from original TEBYORK code ; bcf LEDBit ; Delay 0.5 Second with the LED On movlw 24 call PWMExecute btfss STATUS, Z goto Loop ; If NO Carry, then Process BS2 Command ; btfss BS2Active ; #### - Removed from original TEBYORK code ; bsf LEDBit ; Delay 0.5 Second with the LED Off movlw 24 call PWMExecute movlw 1 ; If No Commands, run the Basic Behavior movwf CurrentState Loop ; Loop Here for Each Command. bcf CollideIgnore ; Make Sure Collisions are NOT ignored movlw LOW StartTable addwf CurrentState, w movwf PCL StartTable goto CommandPoll ; = 0x000 - Stopped, waiting for Command goto Behavior1 ; = 0x001 - Behavior1 (Random Motion) goto Behavior2 ; = 0x002 - Behavior2 (Photovore) goto Behavior2 ; = 0x003 - Behavior3 (Photophobe), uses Behavior2 as a base goto Behavior4 ; = 0x004 - Behavior4 (Maze Follower) goto GOForward ; = 0x005 - BS2/I/R Forward Command goto GOReverse ; = 0x006 - BS2/I/R Reverse Command goto GOLeft ; = 0x007 - BS2/I/R Left Command goto GORight ; = 0x008 - BS2/I/R Right Command if (($ & 0x0F00) != 0) error "Initial Table goes beyond first 256 Instructions" endif PAGE ; Page0 Subroutines LEFTADCRead ; Read the Left ADC movlw LeftCDS ; Discharge the Capacitor btfsc IROff iorlw 1 << 5 ; Start Optionally, the IR LED Checking tris PORTC bsf LeftCDSBit ; Put to a High Voltage movlw 100 / 4 ; Charge High for 100 usecs LADRChargeLoop addwf Neg1, w btfss STATUS, Z goto LADRChargeLoop clrf PWMCount ; Use ADCValue as a Counter movlw PORTCTris ; Stop Charging btfsc IROff iorlw 1 << 5 ; Start Optionally, the IR LED Checking tris PORTC LADRCountLoop ; Count time to Discharge incf PWMCount, f btfsc LeftCDSBit goto LADRCountLoop movlw 0x0FC ; Delete 2 LSBs as "noise" andwf PWMCount, f retlw 0 ; Return to Caller RIGHTADCRead ; Read the Left ADC movlw RightCDS ; Discharge the Capacitor tris PORTB bsf RightCDSBit ; Put to a High Voltage movlw 100 / 4 ; Charge High for 100 usecs RADRChargeLoop addwf Neg1, w btfss STATUS, Z goto RADRChargeLoop clrf PWMCount ; Use ADCValue as a Counter movlw LEDTris ; Stop Charging tris PORTB RADRCountLoop ; Count time to Discharge incf PWMCount, f btfsc RightCDSBit goto RADRCountLoop movlw 0x0FC ; Delete 2 LSBs as "noise" andwf PWMCount, f retlw 0 ; Return to Caller PAGE PWMExecute ; Put PWM Execute Here for Call Redirection movwf PWMCount + 3 ; Save the Number of 20 msec Loops movlw PORTCTris ; Enable Wheel Drivers btfsc IROff iorlw 1 << 5 ; And Optionally, the IR LED tris PORTC bcf SaveLeftCollide bcf SaveRightCollide btfsc LeftCollide ; Save the Current Collision Flags for BS2 bsf SaveLeftCollide btfsc RightCollide bsf SaveRightCollide bcf LeftCollide ; Clear the Collision Flags bcf RightCollide clrf PWMCount ; Clear the Lower two Counters for Fast reset clrf PWMCount + 1 PWM20msecLoop ; Repeat here for each 20msec loop movlw 20 ; Loop Simply before Returning to Start movwf PWMCount + 2 clrf LeftCollideCount ; Normally Stop on Collision - Reset Counter clrf RightCollideCount PWM1msecLoop bsf PWMCount, 2 ; PWMCount = 4 bsf PWMCount + 1, 4 ; PWMCount + 1 = 16 PELoop ; Do PWM with I/R LED Operational movlw 4 subwf PWMOnValue, w ; 100% PWM Operation movlw WheelStop btfsc STATUS, C movf WheelValue, w andlw 0x03F ^ (1 << 5) ; LED Bit Turned On movwf PORTC ; Three "Notches" of Operation movlw 3 subwf PWMOnValue, w ; Three "Notch" Operation movlw WheelStop btfsc STATUS, C movf WheelValue, w andlw 0x03F ^ (1 << 5) ; Keep LED Bit On movwf PORTC ; First "Notch" movlw 2 subwf PWMOnValue, w ; Two "Notch" Operation movlw WheelStop btfsc STATUS, C movf WheelValue, w movwf PORTC ; Second Notch" - Note: LED Off movlw 1 subwf PWMOnValue, w ; One "Notch" Operation movlw WheelStop btfsc STATUS, C movf WheelValue, w movwf PORTC ; Keep LED Bit Off btfss LeftIR ; Reflection to the Left? incf LeftCollideCount, f ; Yes, Increment btfss RightIR ; Reflection to the Right? incf RightCollideCount, f goto $ + 1 bcf IRLEDBit ; LED On Again after 13 Cycles btfss LeftIR ; Reflection to the Left? incf LeftCollideCount, f ; Yes, Increment btfss RightIR ; Reflection to the Right? incf RightCollideCount, f nop btfss LeftIR ; Reflection to the Left? incf LeftCollideCount, f ; Yes, Increment btfss RightIR ; Reflection to the Right? incf RightCollideCount, f nop btfss LeftIR ; Reflection to the Left? incf LeftCollideCount, f ; Yes, Increment bsf IRLEDBit ; LED Off Again after 13 Cycles nop btfss RightIR ; Reflection to the Right? incf RightCollideCount, f decfsz PWMCount, f ; Repeat Again? goto PELoop nop ; Time to Exact 13 Cycles per Loop PWMNullLoop ; Repeat of "PELoop" but no IR Operations movlw 4 subwf PWMOnValue, w ; 100% PWM Operation movlw WheelStop btfsc STATUS, C movf WheelValue, w nop ; LED Bit Turned On movwf PORTC ; Three "Notches" of Operation movlw 3 subwf PWMOnValue, w ; Three "Notch" Operation movlw WheelStop btfsc STATUS, C movf WheelValue, w nop ; Keep LED Bit On movwf PORTC ; First "Notch" movlw 2 subwf PWMOnValue, w ; Two "Notch" Operation movlw WheelStop btfsc STATUS, C movf WheelValue, w movwf PORTC ; Second Notch" - Note: LED Off movlw 1 subwf PWMOnValue, w ; One "Notch" Operation movlw WheelStop btfsc STATUS, C movf WheelValue, w movwf PORTC ; Keep LED Bit Off goto $ + 1 ; Match Timings with Above goto $ + 1 goto $ + 1 goto $ + 1 goto $ + 1 goto $ + 1 goto $ + 1 goto $ + 1 goto $ + 1 goto $ + 1 goto $ + 1 nop decfsz PWMCount + 1, f ; Repeat Again? goto PWMNullLoop movlw IRCommandCount ; Increment the Count from the Last IR Bit Received movwf FSR incfsz INDF, f ; Increment the Counter incf INDF, f decf INDF, f ; If Result == 0, Decrement back to 0x0FF clrf FSR ; Make Sure nothing invalid Written ; #### - Put IR CommandCount Increment Code Code Here? btfss LeftIR ; IR Command? goto PEIRRead PEIRGlitch btfss BS2Clock ; BS2 Command? goto PEBS2Read PEBS2Glitch decfsz PWMCount + 2, f ; Repeat for 20 msecs goto PWM1msecLoop movlw 20 ; Look for Collisions subwf LeftCollideCount, f ; Left Collision? btfsc STATUS, C bsf LeftCollide ; Yes, if 20+ Reads subwf RightCollideCount, f ; Right Collision? btfsc STATUS, C bsf RightCollide ; Yes, if 20+ Reads btfss LeftCollide ; Collision to Handle? btfsc RightCollide goto PECollide PECollideGlitch decfsz PWMCount + 3, f ; Repeat the 20 msec Loop goto PWM20msecLoop bsf STATUS, C ; No Problem, Return bsf STATUS, Z PEStop ; Finished, Return movlw WheelStop ; Stop the Wheels movwf PORTC retlw 0 PECollide ; Collision with Something bcf STATUS, C bsf STATUS, Z btfsc CollideIgnore goto PECollideGlitch ; if "CollideIgnore" Flag Set, Ignore Glitch goto PEStop ; Else, Stop PEIRRead ; Read the Incoming IR Signal callm ReadIRBit PEReturnCheck ; If Carry Reset, Return as Error bcf STATUS, Z btfss STATUS, C ; If Carry Reset, Return the Command goto PEStop goto PEIRGlitch PEBS2Read ; Check the BS2 Clock Signal movlw WheelStop ; Stop the Wheels movwf PORTC callm BS2Read bcf STATUS, Z btfss STATUS, C ; If Carry Reset, Return the Command goto PEStop goto PEBS2Glitch PAGE CommandPoll ; Poll the Incoming Command Bits bsf IROff ; Turn OFF Collision Detection movlw WheelStop ; Wheels Aren't Moving movwf WheelValue movlw 1 ; Just Execute for 20 msecs call PWMExecute btfsc STATUS, Z ; Zero Set, Go Back to Loop btfsc BS2Active ; Can LED be Changed? goto CPEnd btfss STATUS, C ; Collision Active? goto CPCollision CPNoCollision ; Neither Bit Set if Here ; btfss LEDBit ; #### - Code taken from original TEBYORK ; bsf LEDBit ; Turn on the LED if it is Not One goto CommandPoll CPCollision ; Some Kind of Collision ; btfsc LEDBit ; #### - Code taken from original TEBYORK ; bcf LEDBit ; #### - Code taken from original TEBYORK goto CommandPoll CPEnd ; Turn Off LED and Return ; btfss BS2Active ; #### - Code taken from original TEBYORK ; bsf LEDBit ; #### - Code taken from original TEBYORK goto Loop PAGE GOForward ; Move Robot Forward according to BS2/Remote Control movlw Forward goto GODo GOReverse ; Move Robot in Reverse according to BS2/Remote Control movlw Reverse goto GODo GOLeft ; Turn Robot Left according to BS2/Remote Control movlw Left goto GODo GORight ; Turn Robot Right according to BS2/Remote Control movlw Right GODo ; Start Running According to I/R Control movwf WheelValue ; Save the Operational Value bsf IROff ; Turn OFF Collision Detection bsf CollideIgnore ; Turn off Collision Detection for Forward/Back movlw 7 ; Repeat 20 msec Loop 7x for 140 msec delay call PWMExecute ; If Repeat Character Received in PWMExecute, then Continue btfsc STATUS, Z clrf CurrentState ; Pass Along Operation Change Command (if not Finished) goto Loop ; Upon Return Loop Around for Next Command PAGE ; Behaviors Behavior1 ; Go Forward Two Seconds, Stop 1/2 Second ; Turn Right for 1 Second, Stop for 1/2 Second Repeat clrf Flag ; Make Sure no Triggers Set B1LoopReturn1 movlw Forward ; Make Motors Run Forward movwf WheelValue movlw 100 ; Execute for 2 Seconds call PWMExecute ; Execute the Loop movlw 0 ; Record where Call Came From btfss STATUS, Z goto Loop ; Zero Set, Go Back to Loop btfss STATUS, C goto B1Collision ; Carry Reset, Collision movlw WheelStop movwf WheelValue bsf CollideIgnore movlw 15 call PWMExecute ; Sit for 300 msec Polling BS2/IR bcf CollideIgnore btfss STATUS, Z goto Loop ; Command Received, Handle it movlw Left ; Turn Left... movwf WheelValue movlw 38 call PWMExecute ; Execute the Loop for 760 msec movlw 1 ; Record where Call Came From btfss STATUS, Z goto Loop ; Zero Set, Go Back to Loop btfss STATUS, C goto B1Collision ; Carry Reset, Collision B1LoopReturn2 movlw WheelStop movwf WheelValue bsf CollideIgnore movlw 15 call PWMExecute ; Sit for 300 msec Polling BS2/IR bcf CollideIgnore btfss STATUS, Z goto Loop ; Command Received, Handle it goto Behavior1 ; Repeat the Behavior B1Collision ; Handle the Exception movwf ReturnIndex ; Record Where to Return (if Appropriate) ; btfss BS2Active ; #### - Code taken from original TEBYORK ; bcf LEDBit ; Turn on the LED Indicating Collision movlw WheelStop movwf WheelValue bsf CollideIgnore movlw 20 call PWMExecute ; Sit for 400 msec Polling BS2/IR btfss STATUS, Z goto Loop ; Command Received, Handle it movlw Reverse ; Make Motors Run Backwards movwf WheelValue movlw 50 call PWMExecute ; Drive the Wheels in reverse for 1 Second btfss STATUS, Z goto Loop movlw WheelStop movwf WheelValue movlw 20 call PWMExecute ; Sit for 400 msec Polling BS2/IR btfss STATUS, Z goto Loop ; Command Received, Handle it movlw Left ; Turn Away from the Collision btfss LeftCollide movlw Right movwf WheelValue movlw 30 call PWMExecute ; Execute the Loop for 600 msecs btfss STATUS, Z goto Loop ; btfss BS2Active ; #### - Code taken from original TEBYORK ; bsf LEDBit ; Turn off the LED Indicating Collision Finished B1HandlerEnd bcf CollideIgnore ; Turn off Collide Ignore decfsz ReturnIndex, w ; Jump back to Correct Execution goto B1LoopReturn1 goto B1LoopReturn2 ; If "ReturnIndex" was "1" to begin with PAGE Behavior2 ; Photovore - Look for light and go towards it bcf IROff ; Allow Collision Detection call LEFTADCRead ; Read the CDS Cell Light Level movf PWMCount, w ; Save the Left Value movwf i call RIGHTADCRead movf PWMCount, w subwf i, w ; Is "i" or "j" Greater? btfsc STATUS, Z goto B2Forward ; Both are Equal, Just go Forward movlw Left btfsc CurrentState, 0 ; Behavior 2 or 3? goto B2toB3 btfsc STATUS, C movlw Right goto B2Turn B2toB3 ; Avoid the Light btfss STATUS, C movlw Right B2Turn movwf WheelValue ; Save the new Wheel Position movlw 4 ; Turn for 80 msecs call PWMExecute btfss STATUS, Z goto Loop ; Zero Set, Go Back to Loop btfss STATUS, C goto B2Stop ; Collision - Stop Operation movlw WheelStop movwf WheelValue bsf CollideIgnore movlw 15 call PWMExecute ; Sit for 300 msec Polling BS2/IR bcf CollideIgnore btfss STATUS, Z goto Loop ; Command Received, Handle it B2Forward movlw Forward ; Go Forward movwf WheelValue movlw 4 ; Go Forward for 80 msecs call PWMExecute btfss STATUS, Z goto Loop ; Zero Set, Go Back to Loop btfss STATUS, C goto B2Stop ; Collision - Stop Operation movlw WheelStop movwf WheelValue bsf CollideIgnore movlw 15 call PWMExecute ; Sit for 300 msec Polling BS2/IR bcf CollideIgnore btfss STATUS, Z goto Loop ; Command Received, Handle it goto Behavior2 ; Loop Around B2Stop ; Collision - Stop clrf CurrentState goto Loop PAGE Behavior4 ; Maze Follower - Keep one Whisker on a Wall bcf IROff ; Allow Collision Detection movlw WheelStop movwf WheelValue bsf CollideIgnore movlw 4 call PWMExecute ; Sit for 80 msec Polling BS2/IR btfss STATUS, Z goto Loop ; Command Received, Handle it movlw Forward movwf WheelValue movlw 4 ; Go Forward for 80 msecs call PWMExecute ; Note, this is to "look" for btfss STATUS, Z goto Loop ; New Command Received ; - Run at FULL Speed Behavior4Block ; Keep Turning Left if Something in Front movlw Right ; Turn on Left or Right Depending on Sensor btfss LeftCollide ; If Left Collision, Turn Left Immediately btfsc RightCollide ; If Right Collision move to the Left to get away movlw Left ; If Right Whisker not hit, then Turn Right Into it movwf WheelValue movlw 4 ; Turn for 80 msecs call PWMExecute btfss STATUS, Z goto Loop ; New Command Received btfsc LeftCollide ; Something Still Blocking to the Left? goto Behavior4Block ; Yes, Keep Turning Left goto Behavior4 ; Repeat forever PAGE org 0x0200 ; Page1 Subroutines BS2Read ; Read the BS/2 Value Coming In clrf FSR ; Make Sure FSR is Reset movlw 8 ; Count Number of Bits to Read movwf i movlw 3 ; Wait 3.8 msecs for Data to become active movwf k clrf j BS2RLoop1 ; Wait for Data to become Active btfsc BS2Clock goto BS2RSkip3 decfsz j, f ; Wait no more than 3 msecs goto BS2RLoop1 btfsc BS2Clock goto BS2RSkip3 decfsz k, f goto BS2RLoop1 BS2RError ; Nothing Valid Received clrf FSR ; Make Sure FSR is Valid movlw LEDTris ; Make Sure that BS2Data is an Input tris PORTB bsf STATUS, C ; Mark that it is an Error and retlw 0 ; Return BS2RSkip3 movlw 4 movwf j BS2RLoop3 ; Wait no more than 15 msecs for Clock Pulse Active btfss BS2Clock goto BS2RSkip4 decfsz j, f goto BS2RLoop3 goto BS2RError BS2RSkip4 ; Read the Data Bit bcf STATUS, C btfsc BS2Data bsf STATUS, C movlw BS2Byte ; Setup Address for BS2 Byte movwf FSR rrf INDF, f ; Shift in the Bit clrf FSR decfsz i, w ; At the Last Bit? goto BS2RSkip2 ; - No... movlw BS2Byte movwf FSR movlw 16 ; Is this a Return Value? subwf INDF, w clrf FSR btfsc STATUS, C goto BS2RSkip5 ; Yes, Won't Be Seeing any High Value BS2RSkip2 movlw 100 movwf j BS2RLoop2 ; Wait no more than 500 usecs for Clock High btfsc BS2Clock goto BS2RSkip5 decfsz j, f goto BS2RLoop2 goto BS2RError BS2RSkip5 ; Finished with the Bit decfsz i, f ; Read Another Bit? goto BS2RSkip3 bsf BS2Active ; Note that BS2 is Active, Disable other Operations movlw BS2Byte ; Process the Byte that was Read in movwf FSR movlw 22 ; Invalid Command? subwf INDF, w btfsc STATUS, C goto BS2RError ; > 21, Invalid movlw 9 ; Look for Primary Commands subwf INDF, w btfss STATUS, C goto BS2PrimaryCommand movlw 16 ; Look for Secondary Commands subwf INDF, w btfss STATUS, C goto BS2SecondaryCommand movf INDF, w ; Get the command to Process clrf FSR ; Return to Bank0 xorlw 16 ; Return PWM Value? btfsc STATUS, Z goto BS2SCReturnPWM xorlw 17 ^ 16 ; Return CurrentState? btfsc STATUS, Z goto BS2SCReturnCS xorlw 18 ^ 17 ; Return Whisker Values? btfsc STATUS, Z goto BS2SCReturnWhiskers xorlw 19 ^ 18 ; Return Left CDS Value? btfsc STATUS, Z goto BS2SCReturnCDSL xorlw 20 ^ 19 ; Return Right CDS Value? btfsc STATUS, Z goto BS2SCReturnCDSR BS2SCReturnButton ; Any Button Waiting? movlw LastButton ; Get the Button movwf FSR movf INDF, w clrf INDF ; Clear the Button, it has been read goto BS2ReturnCommand BS2SCReturnPWM movf PWMOnValue, w ; Return the PWM On Value goto BS2ReturnCommand BS2SCReturnCS movf CurrentState, w ; Return the PWM On Value goto BS2ReturnCommand BS2SCReturnWhiskers swapf Flag, w ; Return the saved Whiskers Value andlw 3 ; JUST return the Whisker's Value goto BS2ReturnCommand BS2SCReturnCDSL ; Read the Left CDS Cell movlw LeftCDS ; Copy the Subroutine btfsc IROff iorlw 1 << 5 ; Start Optionally, the IR LED Checking tris PORTC bsf LeftCDSBit ; Put to a High Voltage movlw 100 / 4 ; Charge High for 100 usecs BS2SCLADRChargeLoop addwf Neg1, w btfss STATUS, Z goto BS2SCLADRChargeLoop clrf i movlw PORTCTris ; Stop Charging btfsc IROff iorlw 1 << 5 ; Start Optionally, the IR LED Checking tris PORTC BS2SCLADRCountLoop ; Count time to Discharge incf i, f btfsc LeftCDSBit goto BS2SCLADRCountLoop movlw 0x0FC ; Delete 2 LSBs as "noise" andwf i, w goto BS2ReturnCommand BS2SCReturnCDSR ; Read the Right CDS Cell movlw RightCDS ; Copy the Subroutine tris PORTB bsf RightCDSBit ; Put to a High Voltage movlw 100 / 4 ; Charge High for 100 usecs BS2SCRADRChargeLoop addwf Neg1, w btfss STATUS, Z goto BS2SCRADRChargeLoop clrf i ; Use ADCValue as a Counter movlw LEDTris ; Stop Charging tris PORTB BS2SCRADRCountLoop ; Count time to Discharge incf i, f btfsc RightCDSBit goto BS2SCRADRCountLoop movlw 0x0FC ; Delete 2 LSBs as "noise" andwf i, w BS2ReturnCommand ; Data to be Returned clrf FSR ; Save the Data to be Returned bsf FSR, 4 bsf FSR, 6 ; FSR = 0x050 movwf INDF clrf FSR ; Restore Where Everything Is movlw SERIALOut ; Output Data on RB4 tris PORTB movlw 8 ; Count Number of Bits to Read movwf i movlw 2 ; Wait for Reply no more than 2.5 msecs movwf k clrf j BS2RLoop4 ; Wait for Data to become Active btfsc BS2Clock goto BS2RSkip6 decfsz j, f ; Wait no more than 3 msecs goto BS2RLoop4 decfsz k, f goto BS2RLoop4 goto BS2RError BS2RSkip6 ; Output the Data Bit movlw BS2Byte movwf FSR rrf INDF, f ; Put the Data Bit into Carry clrf FSR movf PORTB, w ; Set/Reset the Appropriate Bit iorlw 1 << 4 ; Assume Set btfss STATUS, C andlw 0x03F ^ (1 << 4) ; If Carry Reset, Reset movwf PORTB movlw 4 movwf j BS2RLoop5 ; Wait no more than 15 msecs for Clock Pulse Active btfss BS2Clock goto BS2RSkip7 decfsz j, f goto BS2RLoop5 goto BS2RError BS2RSkip7 ; Wait for Line Low to End movlw 200 movwf j BS2RLoop6 ; Wait no more than 1 msec for Clock High btfsc BS2Clock goto BS2RSkip8 decfsz j, f goto BS2RLoop6 goto BS2RError BS2RSkip8 ; More Bits to Process? decfsz i, f goto BS2RSkip6 ; Yes, Loop Around Again goto BS2RError ; Finished, Return and Keep Processing BS2PrimaryCommand ; Change the Command Operation movf INDF, w ; Get the New Command clrf FSR ; Go Back to Bank0 movwf CurrentState bcf STATUS, C ; Note that there is a Change retlw 0 BS2SecondaryCommand movf INDF, w ; Get the Command clrf FSR ; Go Back to Bank0 xorlw 9 ; Turn LED On? btfsc STATUS, Z goto BS2SCLEDOn xorlw 10 ^ 9 ; LED Off? btfsc STATUS, Z goto BS2SCLEDOff xorlw 11 ^ 10 ; Stop Motor? btfsc STATUS, Z goto BS2SCMotorOff xorlw 12 ^ 11 ; Motor 1 Notch? btfsc STATUS, Z goto BS2SCMotor1 xorlw 13 ^ 12 ; Motor 2 Notch? btfsc STATUS, Z goto BS2SCMotor2 xorlw 14 ^ 13 ; Motor 3 Notch? btfsc STATUS, Z goto BS2SCMotor3 BS2SCMotorFull ; Else, Motor is Full On movlw 4 goto BS2SCMotor BS2SCMotor3 ; Motor is 3 Notches movlw 3 goto BS2SCMotor BS2SCMotor2 ; Motor is 2 Notches movlw 2 goto BS2SCMotor BS2SCMotor1 ; Motor is 1 Notche movlw 1 goto BS2SCMotor BS2SCMotorOff ; Motor is Off movlw 0 BS2SCMotor ; Save the Motor Value clrf FSR movwf PWMOnValue goto BS2RError ; Return Without Changing the Operation BS2SCLEDOn ; #### - Function now a "NOP" clrf FSR ; bcf LEDBit ; #### - Code taken from original TEBYORK goto BS2RError BS2SCLEDOff ; #### - Function now a "NOP" clrf FSR ; bsf LEDBit ; #### - Code taken from original TEBYORK goto BS2RError PAGE ReadIRBit ; Read the I/R Word that is Coming In movlw 33 ; Read 33 Bits before checking value movwf k movlw HIGH ((7500 / 7) + 256) ; Do a "Glitch" Check, Poll for 7.5 msecs and movwf Dlay + 1 ; if no delay through, return movlw LOW ((7500 / 7) + 256) movwf Dlay RIRBLoop ; Loop While Line is High btfsc LeftIR goto RIRBError ; "Glitch", Skip Over decf Dlay, f ; Check What's Coming in btfsc STATUS, Z decfsz Dlay + 1, f goto RIRBLoop RIRBLoop1 ; Wait while Start is Low to Read First Bit btfss LeftIR goto RIRBLoop1 clrf i ; Use "i/j" as the Actual Counter clrf j movlw HIGH ((6000 / 14) + 256) movwf Dlay + 1 movlw LOW ((6000 / 14) + 256) movwf Dlay RIRBLoop2 ; Loop While Line is High goto $ + 1 goto $ + 1 btfss LeftIR goto RIRBHighEnd ; Line Low Again incf i, f ; Increment the count btfsc STATUS, Z incf j, f decf Dlay, f ; Check What's Coming in btfsc STATUS, Z decfsz Dlay + 1, f goto RIRBLoop2 RIRBError ; Nothing Changes (Not Neccesarily an Error) clrf FSR ; Keep Executing After Subroutine ; btfss BS2Active ; #### - Code taken from original TEBYORK ; bsf LEDBit ; Turn OFF LED bsf STATUS, C ; Mark the Error retlw 0 RIRBHighEnd ; Have High Value movf j, w btfsc STATUS, Z goto RIRBHEShort ; Look at it being a Data Bit decfsz j, w ; Start Bit? goto RIRBError ; Value should be > 512 and < 768 movf k, w ; First Bit? xorlw 33 btfss STATUS, Z goto RIRBError ; Outside range, bad goto RIRBBitEnd ; Don't Care about the Bit RIRBHEShort ; Check Range for Bits movf i, w addwf OneFirst, w ; Look for High between 400 - 700 usecs addwf OneSecond, w btfsc STATUS, C goto RIRBBitEnd ; Set Carry will Save the Bit movf i, w addwf ZeroFirst, w ; Look for High between 1.5 - 1.7 msecs addwf ZeroSecond, w btfss STATUS, C goto RIRBContinueCheck bcf STATUS, C ; Zero, Save it goto RIRBBitEnd ; Set Carry will Save the Bit RIRBContinueCheck ; Check for 'i' between 2.1 and 2.3 msecs movf i, w addwf ContFirst, w ; Look for High between 2.1 - 2.3 msecs addwf ContSecond, w btfss STATUS, C goto RIRBError ; Unknown Value, Glitch clrf i ; Use "i/j" as the Actual Counter clrf j movlw HIGH ((1000 / 14) + 256) movwf Dlay + 1 movlw LOW ((1000 / 14) + 256) movwf Dlay RIRBLoop3 ; Loop While Line is Low goto $ + 1 goto $ + 1 btfsc LeftIR goto RIRBContEnd ; Line Low Again incf i, f ; Increment the count btfsc STATUS, Z incf j, f decf Dlay, f ; Check What's Coming in btfsc STATUS, Z decfsz Dlay + 1, f goto RIRBLoop3 btfss LeftIR ; Wait for Line to Go High Again goto $ - 1 goto RIRBError ; Unknown Character/Packet RIRBContEnd movf j, w ; Make Sure Didn't Loop Around btfss STATUS, Z goto RIRBError movf i, w addwf OneFirst, w ; Look for High between 400 - 700 usecs addwf OneSecond, w btfss STATUS, C goto RIRBError ; Not at around 600 usecs, then Error movf k, w ; At First Bit? xorlw 33 btfss STATUS, Z goto RIRBError ; btfss BS2Active ; #### - Code taken from original TEBYORK ; bcf LEDBit ; Turn on LED movf CurrentState, w ; Look for Matches xorlw 5 ; Forward? btfsc STATUS, Z goto RIRBContDo xorlw 6 ^ 5 ; Reverse? btfsc STATUS, Z goto RIRBContDo xorlw 7 ^ 6 ; Left? btfsc STATUS, Z goto RIRBContDo xorlw 8 ^ 7 ; Right? btfss STATUS, Z goto RIRBError RIRBContDo ; Continue the Operation movlw 10 ; Reset to 200 msecs movwf PWMCount + 3 goto RIRBError ; Don't Change the Value RIRBBitEnd ; btfss BS2Active ; #### - Code taken from original TEBYORK ; bcf LEDBit ; Turn on LED movlw IRTable ; Point to the I/R Table for Data Save movwf FSR rlf INDF, f ; Shift in the Bit incf FSR, f rlf INDF, f incf FSR, f rlf INDF, f incf FSR, f rlf INDF, f clrf FSR ; Keep Working through decfsz k, f ; Do another bit? goto RIRBLoop1 movlw IRTable ; Point to the I/R Table for Data Save movwf FSR movf INDF, w incf FSR, f ; Point to the Next Value and Look for Complement xorwf INDF, w xorlw 0x0FF btfss STATUS, Z goto RIRBError ; Not there, Error incf FSR, f ; Next is Zero movf INDF, w btfss STATUS, Z goto RIRBError incf FSR, f ; Last Should be 0x0FF comf INDF, w ; Complement should be Zero btfss STATUS, Z goto RIRBError movlw 3 ; Restore FSR to Start of Bits subwf FSR, f movf INDF, w ; Get the IR Command clrf FSR movwf IRCommand movlw IRCommandCount ; Reset the IR Command Counter movwf FSR movf INDF, w ; Save the IRCommandCounter Value clrf INDF incf FSR, f movwf INDF ; In IRCommandDlay Variable clrf FSR ; Keep FSR Clear btfss LeftIR goto $ - 1 ; Wait for Line to Go High Again movf IRCommand, w ; Process the IR Command Appropriately xorlw Stop_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Stop_But ^ Stop_But ; #### - Added for SumoBot btfss STATUS, Z goto RIBRNotStop movlw 0 goto RIBRNewCurrentState ; Stop and Just Sit There RIBRNotStop movf IRCommand, w ; #### - Added for SumoBot xorlw Forward_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Forward_But ^ Forward_But ; #### - Added for SumoBot btfss STATUS, Z goto RIBRNotForward movlw 5 goto RIBRNewCurrentState RIBRNotForward movf IRCommand, w ; #### - Added for SumoBot xorlw Reverse_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Reverse_But ^ Reverse_But ; #### - Added for SumoBot btfss STATUS, Z goto RIBRNotReverse movlw 6 goto RIBRNewCurrentState RIBRNotReverse movf IRCommand, w ; #### - Added for SumoBot xorlw TurnLeft_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_TurnLeft_But ^ TurnLeft_But ; #### - Added for SumoBot btfss STATUS, Z goto RIBRNotTurnLeft movlw 7 goto RIBRNewCurrentState RIBRNotTurnLeft movf IRCommand, w ; #### - Added for SumoBot xorlw TurnRight_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_TurnRight_But ^ TurnRight_But ; #### - Added for SumoBot btfss STATUS, Z goto RIBRNotTurnRight movlw 8 goto RIBRNewCurrentState RIBRNotTurnRight ; Look for New Button Presses movlw IRCommandDlay ; #### - Added for SumoBot movwf FSR incf INDF, w ; Does IRCommandDlay == 0x0FF btfss STATUS, Z goto RIRBError ; No, Ignore it clrf FSR RIBRButtonDebounced ; Button Pressed has been debounced movf IRCommand, w ; #### - Added for SumoBot xorlw Behavior1_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Behavior1_But ^ Behavior1_But ; #### - Added for SumoBot btfss STATUS, Z goto RIBRNotBehavior1 movlw 1 ; Start up Behavior1 goto RIBRNewCurrentState RIBRNotBehavior1 movf IRCommand, w ; #### - Added for SumoBot xorlw Behavior2_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Behavior2_But ^ Behavior2_But ; #### - Added for SumoBot btfss STATUS, Z goto RIBRNotBehavior2 movlw 2 goto RIBRNewCurrentState RIBRNotBehavior2 movf IRCommand, w ; #### - Added for SumoBot xorlw Behavior3_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Behavior3_But ^ Behavior3_But ; #### - Added for SumoBot btfss STATUS, Z goto RIBRNotBehavior3 movlw 3 goto RIBRNewCurrentState RIBRNotBehavior3 movf IRCommand, w ; #### - Added for SumoBot xorlw Behavior4_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Behavior4_But ^ Behavior4_But ; #### - Added for SumoBot btfss STATUS, Z goto RIBRButtonCheck movlw 4 RIBRNewCurrentState ; Update the Current State and Wait for Next I/R movwf CurrentState ; btfss BS2Active ; #### - Code taken from original TEBYORK ; bsf LEDBit ; Turn OFF LED bcf STATUS, C retlw 0 RIBRButtonCheck movf IRCommand, w ; #### - Added for SumoBot xorlw Button1_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Button1_But ^ Button1_But ; #### - Added for SumoBot btfss STATUS, Z goto $ + 3 movlw 1 goto RIRBNewButton movf IRCommand, w ; #### - Added for SumoBot xorlw Button2_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Button2_But ^ Button2_But ; #### - Added for SumoBot btfss STATUS, Z goto $ + 3 movlw 2 goto RIRBNewButton movf IRCommand, w ; #### - Added for SumoBot xorlw Button3_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_Button3_But ^ Button3_But ; #### - Added for SumoBot btfss STATUS, Z goto RIRBSpeedChange movlw 3 RIRBNewButton ; Last Button to be Pressed clrf FSR bsf FSR, 4 ; Store Last Button at 0x030 bsf FSR, 5 movwf INDF clrf FSR ; btfss BS2Active ; #### - Code taken from original TEBYORK ; bsf LEDBit ; Turn OFF LED bcf STATUS, C retlw 0 RIRBSpeedChange movf IRCommand, w ; #### - Added for SumoBot xorlw SpeedUp_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_SpeedUp_But ^ SpeedUp_But ; #### - Added for SumoBot btfss STATUS, Z goto RIRBSlowDownTest movf PWMOnValue, w ; At Maximum Speed? xorlw 4 btfsc STATUS, Z goto RIRBError incf PWMOnValue, f ; No, Can Increment it goto RIRBError RIRBSlowDownTest movf IRCommand, w ; #### - Added for SumoBot xorlw SlowDown_But btfss PORTB, 5 ; #### - Added for SumoBot xorlw Red_SlowDown_But ^ SlowDown_But ; #### - Added for SumoBot btfss STATUS, Z goto RIRBError movf PWMOnValue, w ; At Minimum Speed? btfsc STATUS, Z goto RIRBError decf PWMOnValue, f ; No, Can Decrement it goto RIRBError if ($ >= 0x03FE) ; Save Last Instruction for OSCCAL error "Past End of Program Memory endif end