' ' Robby003, Robby avoiding obstacles, using ACS (interrupt) and front light sensors. ' Hevawi 10-07-2004 ' Build: ' 001 2004-07-12, original ' 002 2004-08-11, unused function 'lTstForAcsMax' removed, text updated ' ' Robby starts with a beep for a few seconds waiting for the ACS interrupt system to have started. ' If Robby would start immediately, he could collide with an object. ' Robby starts with sending low-power infrared pulses (ACS_LO). ' If no objects are detected the infrared power will increase to ACS_HI and then to ACS_MAX. ' If an object is detected, Robby will decrease the infrared power from ACS_MAX to ACS_HI and then to ACS_LO. ' In each of the 3 ACS modes Robby drives at different maximum speeds. Fastest in ACS_MAX and slowest in ACS_LO. ' If speed is changed, it is done slowly, not abrupt. ' If Robby detects an object left or right in ACS_LO mode, only then he will drive backwards ' until none of both objects are detected anymore. ' When driving backwards, if the left object was detected first, then the right track will be ' blocked and vice versa, to make Robby turn around. ' If both front light sensors detect a very low light level, Robby will handle this the same way ' as when detection objects in ACS_LO mode. So driving reverse one track. ' Led indications ' Led1: left track decreasing reverse speed or increasing forward speed (purpose: left track going forward) ' Led2: left track decreasing forward speed or increasing reverse speed (purpose: left track going reverse) ' Led3: right track decreasing forward speed or increasing reverse speed (purpose: right track going reverse) ' Led4: right track decreasing reverse speed or increasing forward speed (purpose: right track going forward) ' Sound indications ' Beeping when detecting object or no object, left or right. ' Beeping when changing ACS mode ' ' User changable constants: ' Maximum speed in each different ACS mode ' Minimum speed in each different ACS mode ' Number of received ACS infrared pulses before changing ACS mode to less powerfull mode (object approach) ' Number of not received ACS infrared pulses (by mainloop counter counting) before changing ACS mode to more powerfull ' ACS mode (no object detection) ' ' Example. ' Robby starts in mode ACS_LO, then goes to ACS_HI and then goes to ACS_MAX. If no object is detected Robby will ' slowly (5 units per main loop) increase speed to 160 (ACS_MAX) if 2 consequtive infrared hits are detected Robby switches to ' ACS_HI and sets maximum speed to 110, decreasing speed each mainloop with 5 until 110 is reached. If 3 consequtive infrared ' hits are detected in ACS_HI mode then Robby will switch to ACS_LO, slowly decreasing speed with 5 units per mainloop run ' until speed 80 is reached (which is almost or just stand still). With ACS_MAX and ACS_HI there's almost no difference ' in hits between left object or right object detection. So in these modes, individual track speed control, has very ' little use. ' In ACS_LO mode detecting any left object will slowly (5 units per mainloop) slow down the right track speed with 5 units ' per mainloop to speed 0 and then reverse track direction and slowly(5 units per mainloop) increase reverse speed to ' speed 100. This same story applies to detecting right object and controlling left track. This value of 100 is an experimental ' speed value, as Robby will not turn around when the other track is blocked with a speedvalue of less than 100. ' If the second track wants to reverse, that track is blocked, to let Robby make a turn. ' While turning reverse, if no objects are detected anymore for 25 mainloop counts, Robby will switch to mode ' ACS_HI. But before that, while maximum reverse speed is 100 and mainloop increase is 5, it will tak 100/5 = 20 mainloops ' before reverse speed is zero and another about 10 (times 5) loops to make Robby have enough power (10*5=50) to go forward. ' So Robby needs 25 mainloops before the one reversing tracks goes forward. (Remember: the other track was blocked, so only needs ' about 5 mainloop runs to go forward). Therefore the ACS_LO non-object-detection hitcount (mainloop counts) was set to 40. ' So when turning reverse, and detecting no object, Robby will have both tracks driving forward in 25 mainloop runs and stay in ' ACS_LO mode until mainloop run 40 (if no object detected). ' Robby will artificially set the collision-detection flags (user defined flags) when it's front light-sensors detect a ' light level on one or on both light sensors below value 10. This normally will help Robby avoiding objects when ' in ACS_LO mode being very close to objects. ' ' I tried to comment the program code behind each code line, to decrease the amount of sourcelines, but it got all ' to compressed/unreadable. So I put comment with each code line above the code line. It may seem a little bit too much ' or overdone. But my experience is that when reading my own written program months later, if not commented the way I did, ' it will cost many hours to understand my own programming. ' I tried to maintain as must as possible the 'Robby Basic' programming style. ' For the reason of not mixing up variables, constants, bits, bytes and words I named my own variables with a prefix and ' in kind of triplets words like bAaaBbbCcc, for example a byte 'bReqEngLftSpd' means: byte Requested-Engine-Left-Speed ' I named flags/bits like Robby is normally used to, like FLAGNAME_F, with the _F extension (although it's not my kind of programming) ' I like naming goto/gosub labels starting with an 'l' from (l)abel, also using triplets. ' ' Unused Robby defines are left in the source code but are outcommented ' ' I know the program could be more structured, but it will cost me too much time, so I leave it as it is. ' ' If you get anoyed by the many 'Beeps' of course you may remove them... ' '#################################################################################################### ' Each time when SYS COMNAV_STATUS is executed, the next thing happens: ' First , the left IR-Led, on the small Robby front print, transmits a burst of 15 pulses ' Second, the right IR-Led, on the small Robby front print, transmits a burst of 15 pulses ' The IR-Sensor in the middle, on the top-print, will sense the signals a fraction of a second later. ' The subsystem therefore knows the difference between signals reflected from a left-IR-led transmission ' or signals reflected from the right-IR-led transmission. ' If the IR-receiver detects more than 6 reflecting pulses it sets the corresponding ' left or right ACSL_F/ACSR_F bit-flag, else it clears the corresponding flag ' The pulses are transmitted at 36KHz frequency. ' ' +--+ +--+ +--+ // +--+ +--+ +--+ ' |01| |02| |03| // |13| |14| |15| <=== LEFT IR-TRANSMITTER-LED ' ---+ +--+ +--+ +--//--+ +--+ +--+ +---------------------------------------------- ' ' +--+ +--+ +--+ // +--+ +--+ +--+ ' RIGHT IR-TRANSMITTER-LED ===> |01| |02| |03| // |13| |14| |15| ' ----------------------------------------------+ +--+ +--+ +--//--+ +--+ +--+ +--- ' ' +--+ +--+ +--+ // +--+ +--+ +--+ +--+ +--+ +--+ // +--+ +--+ +--+ ' |01| |02| |03| // |13| |14| |15| |01| |02| |03| // |13| |14| |15| <=== TOP IR-RECEIVER (TSOP1836) ' -----+ +--+ +--+ +--//--+ +--+ +--+ +-----+ +--+ +--+ +--//--+ +--+ +--+ +--- '#################################################################################################### ' Program remarks: ' - The left IR-object-detection flag (ACSL_L) controls the right engine. ' - The right IR-object-detection flag (ACSR_L) controls the left engine. ' ' When many different situations may occur, ' and each situation needs a different reaction, ' then so called 'state-machines' are a good solution ' For example: what to do if Robby's... ' ... right engine is .... ' driving forward/driving reverse/has stopped ' ... and left object is ... ' detected/not-detected ' These make 3 * 2 = 6 different situations for the left-object/right-engine combination. ' And another 6 for the right-object/left-engine combination, making 12 different situations to analyse. ' Maybe some situations need the same actions and could be combined, but best ' method is to work them out (all 12) one by one. ' ' The engine control actions are divided into 4 sections. ' 1. left object detected ' 2. no left object detected ' 3. right object detected ' 4. no right object detected ' each of this 4 sections contains 3 different states ' 1. Robby moving forward ' 2. Robby stopped ' 3. Robby moving reverse ' ' When using non-interrupt mode just a SYS COMNAV_STATUS command ' in the mainloop will do. ' But why doing easy if difficult is possible? ' It's a challenge to use interrupt mode, making object detection ' sensing times, independend of the mainloop length, or processor speed ' and get object detection state at a predifined timer rate. ' ' So interrupt mode is used. ' Only if an object is detected, left or right, the ACSL_F/ACSR_F flags ' are changed by the interrupt function. ' If left and right objects are detected and Robby is driving reverse, ' then if no object is detected anymore, then no interrupts will occur and ' both ACS flags will remain set. So Robby keeps driving reverse. ' Therefore ACSL_F and ACSR_F must be cleared after having analysed them. ' ' But....somehow, when clearing both ACS flags, each time, at the end ' of the mainloop, the interrupt will occur much less times!!! ' This should never be the case. An interrupt is an interrupt. ' Maybe the internal timer that is used, controlling the interrupt ' occurrence, is influenced by WRITING to (clearing) ACS flags. ' I did a little test using only READING the ACS flags. This did ' NOT influence interrupt occurence time. ' ' To avoid this delay problem, 2 extra flags are used. ' If the mainloop detects an ACSL_F flag being set, then the mainloop ' sets the associated left 'help' flag and immediately cleares the ACSL_F flag. ' At each mainloop run the 'help' flag is checked (not the ACS flag) and ' the 'help' flag is cleared at each run, not the ACS flag. ' The ACSL_F is only cleared once, immediately after it is detected as set. ' (Remember: The ACS_L flag is automatically set by a collision interrupt.) ' This same story applies to the other, right flag, the ASCR_F flag. '#################################################################################################### '-------------------------- '------ I/O PORTS --------- '-------------------------- '- INTERFACE LCD/EXTPORT -- define sdio port[1] define sclio port[3] define strobe port[4] '-- INTERFACE COM/NAV ----- 'define DATALINE port[1] 'define CLOCKLINE port[2] '-------------------------- '------ SENSORS --------- '-------------------------- define LIGHT_L ad[7] define LIGHT_R ad[6] 'define SYS_VOLTS ad[3] 'define CHRG_CURRENT ad[2] 'define SYS_CURRENT ad[1] 'define MIC ad[4] 'define TOUCH ad[5] '--------------------------- '------ DRIVE ------------- '--------------------------- define SPEED_L da[1] define SPEED_R da[2] define FWD_L port[6] 'REV_L renamed to FWD_L define FWD_R port[5] 'REV_R renamed to FWD_R '-------------------------- '---- SYSTEM MEMORY ------- '-------------------------- '--- INTERFACE BUFFER ---- define LBYTE byte[1] define HBYTE byte[2] define SUBCMD byte[3] '---- OPERATION DATA ------ define EXTPORT byte[4] define COM_NAV_PWR_F bit[25] define ACS_PWR_LO_F bit[26] define ACS_PWR_HI_F bit[27] define SNS_PWR_F bit[28] define LED1_F bit[29] define LED2_F bit[30] define LED3_F bit[31] define LED4_F bit[32] define SYSTEM_STATUS byte[5] define ACSL_F bit[33] define ACSR_F bit[34] 'define IR_F bit[35] '-------------------------- '---- USER MEMORY ------- '-------------------------- 'define GP byte[6] 'define MAXLIGHT byte[7] 'define GP1 byte[8] '--- SYSTEMROUTINEN ----------- 'define PLM_SLOW &H01C4 define SYSTEM &H01C9 define COMNAV &H0154 '- ERWEITERTE SYSTEM ROUTINEN - 'define REVR &H0101 'ANTRIEB RECHTS RÜCKWÄRTS 'define REVL &H0106 'ANTRIEB LINKS RÜCKWÄRTS 'define FWDR &H010B 'ANTRIEG RECHTS VORWÄRTS 'define FWDL &H0110 'ANTRIEB LINKS VORWÄRTS 'define ROTR &H0115 'RECHTS DREHEN 'define ROTL &H0119 'LINKS DREHEN 'define REV &H011D 'RÜCKWÄRTS 'define FWD &H0121 'VORWÄRTS define COMNAV_STATUS &H0125 'UPDATED ALLE FLAGS IM STATUS-REGISTER define ACS_LO &H01E1 'ACS POWER LO define ACS_HI &H01E9 ' define ACS_MAX &H01F1 ' 'define SEND_TLM &H014A 'SENDET TELEMETRIE (CH=HBYTE,DATEN=LBYTE) 'define SEND_SPEEDR &H0134 'SENDET TLM KANAL 8,PLM RECHTS 'define SEND_SPEEDL &H013A 'SENDET TLM KANAL 7,PLM LINKS 'define SEND_SYSSTAT &H0144 'SENDET TLM KANAL 0,SYSTEM STATUS '(FLAGS für ACS,FWD/REV, ACS_LO/HI/MAX) '---------------------------------------- ' USER VARIABLES '---------------------------------------- define bReqEngLftSpd byte[6] 'engine left, requested speed define bReqEngRgtSpd byte[7] 'engine rigt, requested speed '---keep-copy-of-actual-speed-while-actual-speed-itself-is-not-accessible--- define bCpyEngLftSpd byte[8] 'Left engine, copy of actual speed define bCpyEngRgtSpd byte[9] 'Right engine, copy of actual speed define bVhcLftTrkMovSts byte[10] 'Vehicle left engine moving status define bVhcRgtTrkMovSts byte[11] 'Vehicle rigt engine moving status define bIfrRflHitCnt byte[12] 'InfraRed reflection hit counter define bIfrNonHitCnt byte[13] 'InfraRed reflection hit counter define bFwdSpdMax byte[14] 'Maximum forward speed define bRevSpdMax byte[15] 'Maximum reverse speed '---------------------------------------- ' USER DEFINES '---------------------------------------- define SPEED_CHANGE 5 'increase/decrease amount of speed define SPEED_MAX 160 'must be multiple of SPEED_CHANGE (else speed increase/decrease error) define SPEED_MIN 40 'must be multiple of SPEED_CHANGE (else speed increase/decrease error) define SPEED_STOP 40 'must be multiple of SPEED_CHANGE (else speed increase/decrease error) define SPEED_MAX_ACS_LO 80 'must be multiple of SPEED_CHANGE (else speed increase/decrease error) define SPEED_MAX_ACS_HI 110 'must be multiple of SPEED_CHANGE (else speed increase/decrease error) define SPEED_MAX_ACS_MAX 160 'must be multiple of SPEED_CHANGE (else speed increase/decrease error) define SPEED_REVERSE_MAX 100 'must be multiple of SPEED_CHANGE (else speed increase/decrease error) define MOV_STS_STP 1 'moving state stopped define MOV_STS_FWD 2 'moving state forward define MOV_STS_REV 3 'moving state reverse define LFT_OBJ_DET_F bit[169] 'byte 22 bit 1 (n=1-8) define RGT_OBJ_DET_F bit[170] 'byte 22 bit 2 (n=1-8) define BLK_LFT_TRK_F bit[171] 'block left track define BLK_RGT_TRK_F bit[172] 'block right track define SYSSTS_ACS_PWR_BITS_CMP &H60 'SYSTEM_STATUS ACS_PWR bits define SYSSTS_ACS_PWR_LO_CMP &H40 'ACS_LO: HI (bit6) is high/inactive and LO (bit5) is low / active! define SYSSTS_ACS_PWR_HI_CMP &H20 'ACS_HI: HI (bit6) is low / active and LO (bit5) is high/inactive! define SYSSTS_ACS_PWR_MAX_CMP &H00 'ACS_MAX: HI (bit6) is low / active and LO (bit5) is low / active! define ACS_MX_IR_REFL_HIT_COUNT_MAX 2 'ACS_MX: infrared reflection hits count before going to ACS_HI mode (speed down) define ACS_HI_IR_REFL_HIT_COUNT_MAX 3 'ACS_HI: infrared reflection hits count before going to ACS_LO mode (speed down) define ACS_LO_IR_REFL_NON_COUNT_MAX 40 'ACS_LO: infrared reflection none count before going to ACS_HI mode (speed up) define ACS_HI_IR_REFL_NON_COUNT_MAX 25 'ACS_HI: infrared reflection none count before going to ACS_MX mode (speed up) define LGT_SNS_COL_LVL 10 'light sensors collision assumption level 'define interrupt handler label interrupt lIntHnd '================================================================================ ' MAIN PROGRAM '================================================================================ 'avoiding multiple execution of the beginning of the program when pressing the start button too long pause 10 'show terminal message (if terminal connected) print print "Started" print '---initialize-engines--- 'init engines (engines off, direction forward) gosub lIniEng 'switch ACS power to on gosub SUBSYS_PWR_ON 'start with miximum IR power sys ACS_LO 'define maximum reachable forward speed bFwdSpdMax = SPEED_MAX_ACS_LO 'define maximum reachable reverse speed bRevSpdMax = SPEED_REVERSE_MAX 'clear infrared reflection hit counter bIfrRflHitCnt=0 'clear infrared non (reflection) hit counter bIfrNonHitCnt=0 'use ACS 'interrupt' mode (not the 'polling' mode) gosub ACS_INT_200 '---init-flags--- 'clear (eventually set) left ACS flags ACSL_F=off 'clear (eventually set) right ACS flags ACSR_F=off 'clear (eventually set) left ACS 'help' flag LFT_OBJ_DET_F=off 'clear (eventually set) right ACS 'help' flag RGT_OBJ_DET_F=off '---init-statemachines--- 'init left-track-movement-statemachine state as 'stopped' bVhcLftTrkMovSts=MOV_STS_STP 'init right-track-movement-statemachine state as 'stopped' bVhcRgtTrkMovSts=MOV_STS_STP '---wait-for-interrupt-to-start--- 'start beep, indicating: Robby is waiting beep 1000,0,0 'wait for at least 1800ms to enable ACS IR-transmission to start (1700ms measured with oscilloscoop) 'else Robby starts driving forward detecting no object...and bending IR-diodes... pause (1800/20) 'beep end, waiting ended beep 0,0,0 'release left track blocking (use with reverse movement) BLK_LFT_TRK_F=off 'release right track blocking (use with reverse movement) BLK_RGT_TRK_F=off '================================================================================ ' MAIN PROGRAM (ENDLESS LOOP) '================================================================================ 'main loop #lMaiLop 'use pause for better detection results (not sure why) pause 2 '================================================================================ ' -0- INITIALISE 'HELP' FLAGS AND EVENTUALLY CLEAR ACS FLAGS '================================================================================ 'reset left-object-detected 'help' flag LFT_OBJ_DET_F=off 'reset rigt-object-detected 'help' flag RGT_OBJ_DET_F=off '---analyse-collision-flags--- 'if left ACS sensor detects no object, then jump if not ACSL_F then goto labAcsNotDetLftObj '---left-hand-object-detected--- 'beep beep 300,1,0 'clear ACS flag ACSL_F=off 'set left-object-detected 'help' flag LFT_OBJ_DET_F=on '---no-left-object-detected--- #labAcsNotDetLftObj 'if right ACS sensor detects object, then jump if not ACSR_F then goto labAcsNotDetRgtObj '---right-hand-object-detected--- 'beep beep 350,1,0 'clear ACS flag ACSR_F=off 'set right-object-detected 'help' flag RGT_OBJ_DET_F=on '---no-rigt-object-detected--- #labAcsNotDetRgtObj '================================================================================ ' CHECK FRONT LIGHT SENSORS FOR A POSSIBLE 'COLLISSION' (DARKNESS) '================================================================================ 'if ACS is not in mode LO, jump if (SYSTEM_STATUS and &H60) <> &H40 then goto lLgtSnsEnd 'if left front-light-sensor or right front-light-sensor value is above the 'collission' level, jump if (LIGHT_L > LGT_SNS_COL_LVL or LIGHT_R > LGT_SNS_COL_LVL) then goto lLgtSnsEnd '#---a-collision-is-assumed--- 'simulate a left object detection LFT_OBJ_DET_F=on 'simulate a right object detection RGT_OBJ_DET_F=on 'light-sensing-end #lLgtSnsEnd '================================================================================ ' CHECK FOR LEFT AND RIGHT ACS OBJECTS COLLISSION '================================================================================ 'if no object detected, skip if (not LFT_OBJ_DET_F) and (not RGT_OBJ_DET_F) then goto lChkNobAcs '================================================================================ ' OBJECT DETECTED, DECREASE ACS POWER AND DECREASE MAXIMUM FORWARD SPEED '================================================================================ '#---object-detected--- #lChkObjAcs 'clear non hit counter bIfrNonHitCnt=0 '#---ACS-MAX-mode--- #lChkObjAcsMax 'if ACS is not in mode MAX, goto mode HI if (SYSTEM_STATUS and &H60) <> &H00 then goto lChkObjAcsHi '#---now-ACS-is-in-mode-MAX--- 'count reflection hits bIfrRflHitCnt=bIfrRflHitCnt+1 'beep beep 400,1,0 'if not maximum hits counted, jump if bIfrRflHitCnt <> ACS_MX_IR_REFL_HIT_COUNT_MAX then goto lChkObjAcsEnd 'clear hit counter bIfrRflHitCnt=0 'switch to ACS_HI sys ACS_HI 'define maximum speed bFwdSpdMax=SPEED_MAX_ACS_HI 'jump goto lChkObjAcsEnd '#---ACS-HI-mode--- #lChkObjAcsHi 'if ACS is not in mode HI, goto mode LO if (SYSTEM_STATUS and &H60) <> &H20 then goto lChkObjAcsLo '#---now-ACS-is-in-mode-HI--- 'count reflection hits bIfrRflHitCnt=bIfrRflHitCnt+1 'beep beep 410,1,0 'if not maximum hits counted, jump if bIfrRflHitCnt <> ACS_HI_IR_REFL_HIT_COUNT_MAX then goto lChkObjAcsEnd 'clear hit counter bIfrRflHitCnt=0 'switch to ACS_LO sys ACS_LO 'define maximum speed bFwdSpdMax=SPEED_MAX_ACS_LO ' bRevSpdMax=SPEED_MAX_ACS_LO 'jump goto lChkObjAcsEnd '#---ACS-LO-mode--- #lChkObjAcsLo 'beep beep 420,1,0 'jump goto lChkObjAcsEnd '---check-object-ACS-end--- #lChkObjAcsEnd 'skip 'no-object-deteced' part goto lChkAcsEnd '================================================================================ ' NO OBJECT DETECTED, INCREASE ACS POWER AND INCREASE MAXIMUM FORWARD SPEED '================================================================================ '#---no-object-detected--- #lChkNobAcs 'clear hit counter bIfrRflHitCnt=0 '#---ACS-LO-mode--- #lChkNobAcsLo 'if ACS is not in mode LO, goto mode HI if (SYSTEM_STATUS and &H60) <> &H40 then goto lChkNobAcsHi '#---now-ACS-is-in-mode-LO--- 'count reflection hits bIfrNonHitCnt=bIfrNonHitCnt+1 'beep beep 420,1,0 'if not maximum hits counted, jump if bIfrNonHitCnt <> ACS_LO_IR_REFL_NON_COUNT_MAX then goto lChkNobAcsEnd 'clear non hit counter bIfrNonHitCnt=0 'switch to ACS_HI sys ACS_HI 'define maximum speed bFwdSpdMax=SPEED_MAX_ACS_HI 'jump goto lChkNobAcsEnd '#---ACS-HI-mode--- #lChkNobAcsHi 'if ACS is not in mode HI, goto mode MAX if (SYSTEM_STATUS and &H60) <> &H20 then goto lChkNobAcsMax '#---now-ACS-is-in-mode-HI--- 'count reflection hits bIfrNonHitCnt=bIfrNonHitCnt+1 'beep beep 410,1,0 'if not maximum hits counted, jump if bIfrNonHitCnt <> ACS_HI_IR_REFL_NON_COUNT_MAX then goto lChkNobAcsEnd 'clear non hit counter bIfrNonHitCnt=0 'switch to ACS_MAX sys ACS_MAX 'define maximum speed bFwdSpdMax=SPEED_MAX_ACS_MAX 'jump goto lChkNobAcsEnd '#---ACS-MAX-mode--- #lChkNobAcsMax 'beep beep 400,1,0 'jump goto lChkNobAcsEnd '---check-no-object-ACS-end--- #lChkNobAcsEnd '--- #lChkAcsEnd 'handle object-detection and track-control gosub lObjDetTrkCtl '---goto-main-loop--- 'goto main loop goto lMaiLop '================================================================================ ' MAIN PROGRAM END '================================================================================ END '================================================================================ ' INTERRUPT HANDLER '================================================================================ #lIntHnd 'transmit first left and then right infrared pulsetrain and put received result values in ACSL_F and ACSR_F SYS COMNAV_STATUS 'return from interrupt return interrupt '================================================================================ ' PRINT SYSTEM REGISTERS '================================================================================ #lPrtSysRgs 'print system registers print "LBYTE=";LBYTE;" HBYTE=";HBYTE;" SUBCMD=";SUBCMD;" EXTPORT=";EXTPORT;" SYSSTS=";SYSTEM_STATUS 'return return '======================================== ' Init engines ' - engines off, direction forward '======================================== #lIniEng 'define engine left, speed bReqEngLftSpd=0 'define engine rigt, speed bReqEngRgtSpd=0 'activate defined speed gosub lSetEnsSpd 'set left engine to forward FWD_L=on 'set rigt engine to forward FWD_R=on 'return return ''======================================== '' Use Anti-Collision-System 'polling' mode, not the 'interrupt' mode ''======================================== '#NO_ACS_INT ' 'request for system status, will be returned in HBYTE ' SUBCMD = 4 : sys COMNAV ' 'clear ACS-interrupt-enable bit, and store in LBYTE ' LBYTE = HBYTE and &HFB ' 'set system mode ' HBYTE = 00 : SUBCMD = 2 : sys COMNAV ''return 'return '======================================== ' Use Anti-Collision-System 'interrupt' mode, not the 'polling' mode '======================================== #ACS_INT_200 'request for system status, will be returned in HBYTE SUBCMD = 4 : sys COMNAV 'set ACS-interrupt-enable bit, and store in LBYTE LBYTE = HBYTE or &H04 'set ACS sensing interval time to 200ms HBYTE = (200/20) 'set system mode SUBCMD = 2 : sys COMNAV 'return return '======================================== ' Set engine Speed ' - 'copy' speed value always reflects real speed value '======================================== #lSetEnsSpd 'save requested left engine speed bCpyEngLftSpd=bReqEngLftSpd 'set left engine to requested speed SPEED_L=bCpyEngLftSpd 'save requested right engine speed bCpyEngRgtSpd=bReqEngRgtSpd 'set right engine to requested speed SPEED_R=bCpyEngRgtSpd 'return return '======================================== ' Set Left Engine One Step Towards Requested Speed X '======================================== #labSetLftEngOneStpTwdReqSpd 'if left engine is at requested speed... exit if (bCpyEngLftSpd = bReqEngLftSpd) then return 'left engine is faster than requested speed: slow down 'left engine is slower than requested speed: speed up if bCpyEngLftSpd > bReqEngLftSpd then bCpyEngLftSpd=bCpyEngLftSpd-SPEED_CHANGE else bCpyEngLftSpd=bCpyEngLftSpd+SPEED_CHANGE 'activate new left engine speed SPEED_L=bCpyEngLftSpd 'return return '======================================== ' Set Right Engine One Step Towards Requested Speed X '======================================== #labSetRgtEngOneStpTwdReqSpd 'if right engine is at requested speed... exit if (bCpyEngRgtSpd = bReqEngRgtSpd) then return 'left engine is faster than requested speed: slow down 'left engine is slower than requested speed: speed up if bCpyEngRgtSpd > bReqEngRgtSpd then bCpyEngRgtSpd=bCpyEngRgtSpd-SPEED_CHANGE else bCpyEngRgtSpd=bCpyEngRgtSpd+SPEED_CHANGE 'activate new rightt engine speed SPEED_R=bCpyEngRgtSpd 'return return '================================================================================ ' Leds switch on/off '================================================================================ #LED1ON LED1_F=on : sys SYSTEM : return #LED1OFF LED1_F=off : sys SYSTEM : return #LED2ON LED2_F=on : sys SYSTEM : return #LED2OFF LED2_F=off : sys SYSTEM : return #LED3ON LED3_F=on : sys SYSTEM : return #LED3OFF LED3_F=off : sys SYSTEM : return #LED4ON LED4_F=on : sys SYSTEM : return #LED4OFF LED4_F=off : sys SYSTEM : return '================================================================================ ' All Leds off '================================================================================ #LEDSOFF '#ledsoff1: make all led bits (bits 4-7) from EXTPORT go low/off and leave bits 0 to 3 unchanged EXTPORT=EXTPORT and &H0F 'send EXTPORT byte to led/pwr control chip sys SYSTEM 'return return '================================================================================ ' SUBSYS POWER ON '================================================================================ #SUBSYS_PWR_ON 'define serial-data signal as output-low sdio=off 'define serial-clock signal as output-low sclio=off 'define strobe signal as output-low strobe=off 'set EXTPORT communication/navigation power on (by setting bit to OFF!) COM_NAV_PWR_F=off 'set EXTPORT refernce-voltage-microphone-power on SNS_PWR_F=on 'send contents of EXTPORT (memory 4) to shiftregister IC I11 sys SYSTEM 'return return '================================================================================ ' OBJECT DETECTION TRACK CONTROL ' needing: bFwdSpdMax, bRevSpdMax '================================================================================ #lObjDetTrkCtl '================================================================================ ' -1- LEFT OBJECT DETECTED, RIGHT TRACK HANDLING '================================================================================ 'if no left object is detected, jump if not LFT_OBJ_DET_F then goto lNotLftObjDet '#---left-object-detected--- 'indicate: left object detected: led 1 off, led 2 on gosub LED1OFF : gosub LED2ON 'if track is not moving forward, jump if bVhcRgtTrkMovSts <> MOV_STS_FWD then goto lRgtTrkNotFwd1 '#---now-driving-forward-and-object-detected--- 'define track end-speed bReqEngRgtSpd = SPEED_STOP 'change speed with one small step gosub labSetRgtEngOneStpTwdReqSpd 'if track has stopped, then set moving-status to 'stopped' if bCpyEngRgtSpd = SPEED_STOP then bVhcRgtTrkMovSts = MOV_STS_STP 'goto end of left-sensor handling function goto lHndLftSnsEnd '---track-not-moving-forward--- #lRgtTrkNotFwd1 'if track has not stopped, jump if bVhcRgtTrkMovSts <> MOV_STS_STP then goto lRgtTrkNotStp1 '#---now-stopped-and-object-detected--- 'if other (left) track is allready in reverse mode, don't execute reverse-right-track, just skip it (blocking right track) if bVhcLftTrkMovSts = MOV_STS_REV then goto lHndLftSnsEnd 'set engine to reverse FWD_R=off 'set track status to reverse bVhcRgtTrkMovSts = MOV_STS_REV 'goto end of left-sensor handling function goto lHndLftSnsEnd '---track-not-stopped--- #lRgtTrkNotStp1 'if track is not moving reverse, jump if bVhcRgtTrkMovSts <> MOV_STS_REV then goto lRgtTrkNotRev1 '#---now-driving-reverse-and-object-detected--- 'define track end-speed bReqEngRgtSpd = bRevSpdMax 'change speed with one small step gosub labSetRgtEngOneStpTwdReqSpd 'goto end of left-sensor handling function goto lHndLftSnsEnd '---track-not-moving-reverse--- #lRgtTrkNotRev1 '--illegal-state,-programming-error--- goto lErrEnd '================================================================================ ' -2- NO LEFT OBJECT DETECTED, RIGHT TRACK HANDLING '================================================================================ '---handle-right-engine-when-no-object-is-detected--- #lNotLftObjDet '#---no-left-object-detected--- 'indicate: no left object detected: led 2 off, led 1 on gosub LED2OFF : gosub LED1ON 'if track is not moving forward, jump if bVhcRgtTrkMovSts <> MOV_STS_FWD then goto lRgtTrkNotFwd2 '#---now-driving-forward-and-no-object-detected--- 'define end-speed bReqEngRgtSpd = bFwdSpdMax 'change speed with one small step gosub labSetRgtEngOneStpTwdReqSpd 'goto end of left-sensor handling function goto lHndLftSnsEnd '---track-not-moving-forward--- #lRgtTrkNotFwd2 'if track has not stopped, jump if bVhcRgtTrkMovSts <> MOV_STS_STP then goto lRgtTrkNotStp2 '#---now-stopped-and-no-object-detected--- 'set engine to forward FWD_R=on 'set track status to forward bVhcRgtTrkMovSts=MOV_STS_FWD 'goto end of left-sensor handling function goto lHndLftSnsEnd '---track-not-stopped--- #lRgtTrkNotStp2 'if track is not moving reverse, jump if bVhcRgtTrkMovSts <> MOV_STS_REV then goto lRgtTrkNotBwd2 '#---now-driving-reverse-and-no-object-detected--- 'define end-speed bReqEngRgtSpd = SPEED_STOP 'change speed with one small step gosub labSetRgtEngOneStpTwdReqSpd 'if track has stopped, then set moving-status to 'stopped' if bCpyEngRgtSpd = SPEED_STOP then bVhcRgtTrkMovSts = MOV_STS_STP 'goto end of left-sensor handling function goto lHndLftSnsEnd '---track-not-moving-reverse--- #lRgtTrkNotBwd2 '--illegal-state,-programming-error--- goto lErrEnd '---left-sensor-handling-function-end--- #lHndLftSnsEnd '================================================================================ ' -3- RIGHT OBJECT DETECTED, LEFT TRACK HANDLING '================================================================================ 'if no right object is detected, jump if not RGT_OBJ_DET_F then goto lNotRgtObjDet 'indicate: right object detected: led 4 off, led 3 on gosub LED4OFF : gosub LED3ON 'if track is not moving forward, jump if bVhcLftTrkMovSts <> MOV_STS_FWD then goto lLftTrkNotFwd1 '#---now-driving-forward-and-object-detected--- 'define track end-speed bReqEngLftSpd = SPEED_STOP 'change speed with one small step gosub labSetLftEngOneStpTwdReqSpd 'if track has stopped, then set moving-status to 'stopped' if bCpyEngLftSpd = SPEED_STOP then bVhcLftTrkMovSts = MOV_STS_STP 'goto end of left-sensor handling function goto lHndRgtSnsEnd '---track-not-moving-forward--- #lLftTrkNotFwd1 'if track has not stopped, jump if bVhcLftTrkMovSts <> MOV_STS_STP then goto lLftTrkNotStp1 '#---now-stopped-and-object-detected--- 'if other (right) track is allready in reverse mode, don't execute reverse-left-track, just skip it (blocking left track) if bVhcRgtTrkMovSts = MOV_STS_REV then goto lHndRgtSnsEnd 'set engine to reverse FWD_L=off 'set track status to reverse bVhcLftTrkMovSts = MOV_STS_REV 'goto end of left-sensor handling function goto lHndRgtSnsEnd '---track-not-stopped--- #lLftTrkNotStp1 'if track is not moving reverse, jump if bVhcLftTrkMovSts <> MOV_STS_REV then goto lLftTrkNotRev1 '#---now-driving-reverse-and-object-detected--- 'define track end-speed bReqEngLftSpd = bRevSpdMax 'change speed with one small step gosub labSetLftEngOneStpTwdReqSpd 'goto end of left-sensor handling function goto lHndRgtSnsEnd '---track-not-moving-reverse--- #lLftTrkNotRev1 '--illegal-state,-programming-error--- goto lErrEnd '================================================================================ ' -4- NO RIGHT OBJECT DETECTED, LEFT TRACK HANDLING '================================================================================ '---handle-right-engine-when-no-object-is-detected--- #lNotRgtObjDet 'indicate: no right object detected: led 3 off, led 4 on gosub LED3OFF : gosub LED4ON 'if track is not moving forward, jump if bVhcLftTrkMovSts <> MOV_STS_FWD then goto lLftTrkNotFwd2 '#---now-driving-forward-and-no-object-detected--- 'define end-speed bReqEngLftSpd = bFwdSpdMax 'change speed with one small step gosub labSetLftEngOneStpTwdReqSpd 'goto end of left-sensor handling function goto lHndRgtSnsEnd '---track-not-moving-forward--- #lLftTrkNotFwd2 'if track has not stopped, jump if bVhcLftTrkMovSts <> MOV_STS_STP then goto lLftTrkNotStp2 '#---now-stopped-and-no-object-detected--- 'set engine to forward FWD_L=on 'set track status to forward bVhcLftTrkMovSts=MOV_STS_FWD 'goto end of left-sensor handling function goto lHndRgtSnsEnd '---track-not-stopped--- #lLftTrkNotStp2 'if track is not moving reverse, jump if bVhcLftTrkMovSts <> MOV_STS_REV then goto lLftTrkNotBwd2 '#---now-driving-reverse-and-no-object-detected--- 'define end-speed bReqEngLftSpd = SPEED_STOP 'change speed with one small step gosub labSetLftEngOneStpTwdReqSpd 'if track has stopped, then set moving-status to 'stopped' if bCpyEngLftSpd = SPEED_STOP then bVhcLftTrkMovSts = MOV_STS_STP 'goto end of left-sensor handling function goto lHndRgtSnsEnd '---track-not-moving-reverse--- #lLftTrkNotBwd2 '--illegal-state,-programming-error--- goto lErrEnd '---right-sensor-handling-function-end--- #lHndRgtSnsEnd 'return return '================================================================================ ' ERROR, END PROGRAM (Only could happen during program development) '================================================================================ #lErrEnd ' 'show error message on terminal (if terminal connected) ' print ' print "Error, illegal statemachine state" ' print ' '---keep-beeping--- ' #lKepBep ' 'beep ' beep 500,1,1 ' beep 520,1,1 ' beep 540,1,1 ' 'goto keep-beeping ' goto lKepBep end