// Scout Tool Sample Program 6: Proximity Sensor // Copyright The LEGO Company - November 18, 1999 //============================================================================== // // Project : ScoutSDK // Unit : Main // Name : ProxSensor // Language : LEGO P-brick Assembler // Date : 99.11.18 // Ver. : 1.00 // Prog. : Gaute Munch // //------------------------------------------------------------------------------ // // Implements an optical proximity sensor using the IR transmitter together with // the build in light sensor. // // Requires "scoutdef.h" for shortcut commands like on, rwd, fwd. // See Scout SDK documentation for formal assembler commands. // //============================================================================== //============================================================================== // Pre-processor directives //============================================================================== #include "ScoutDef.h" //============================================================================== // Defines //============================================================================== // Tasks //------------------------------------------------------------------------------ #define TASK_PROX_MAIN 0 #define TASK_PROX_SOUND 1 #define TASK_PROX_REACT 2 #define TASK_PROX_DRIVER 3 //------------------------------------------------------------------------------ // Global variables and constants //------------------------------------------------------------------------------ #define GV_PROX_VALUE 0 #define GV_PROX_STATE 1 #define K_PROX_STATE_OFF 0 #define K_PROX_STATE_ON 1 #define EVENT_PROX 0x0200 //------------------------------------------------------------------------------ // Parameter variables and values for subroutine calls //------------------------------------------------------------------------------ #define LV_PAR1 10 #define LV_PAR2 11 #define PAR_MOV_SHAKE 3 #define PAR_SHAKE_TIME 1 #define PAR_AVOID_LEFT 0 #define PAR_AVOID_RIGHT 1 #define PAR_AVOID_TIME 50 //------------------------------------------------------------------------------ // Local variables and constants used in TASK_PROX_MAIN //------------------------------------------------------------------------------ #define LV_DRIVE_TIME 10 #define PRT_PROX_MAIN PRT3 #define K_TURN_TIME FR_MS_500 //------------------------------------------------------------------------------ // Local variables and constants used in TASK_PROX_SOUND //------------------------------------------------------------------------------ #define LV_SOUND_FRQ 10 #define PRT_PROX_SOUND PRT2 #define K_FRQ_SCALER 40 #define K_SOUND_DURATION FR_MS_20 #define K_SOUND_REPEAT_TIME FR_MS_500 //------------------------------------------------------------------------------ // Local variables and constants used in TASK_PROX_REACT //------------------------------------------------------------------------------ #define LV_AVOID_SELECT 10 #define PRT_PROX_REACT PRT1 //------------------------------------------------------------------------------ // Local variables and constants used in TASK_PROX_DRIVER //------------------------------------------------------------------------------ #define LV_LIGHT_BEFORE 10 #define LV_LIGHT_AFTER 11 #define LV_LIGHT_DIF 12 #define LV_LOOP_COUNTER 13 #define K_PROX_LIMIT 100 #define K_PROX_LIMIT_HYST K_PROX_LIMIT - 5 #define K_PROX_MESSAGE 255 #define K_PROXLOOP_DELAY FR_MS_50 #define K_MEASURE_DELAY FR_MS_40 //============================================================================== // // Task : TASK_PROX_MAIN // //------------------------------------------------------------------------------ // // Initialises the Stand Alone Program system of the Scout. // //============================================================================== task TASK_PROX_MAIN //------------------------------------------------------------------------------ // Play some sounds and wait a bit //------------------------------------------------------------------------------ plays SND_SWEEP_FAST wait SRC_CON, FR_MS_500 plays SND_SWEEP_FAST wait SRC_CON, FR_SEC_1 //------------------------------------------------------------------------------ // Start program tasks //------------------------------------------------------------------------------ start TASK_PROX_SOUND start TASK_PROX_REACT start TASK_PROX_DRIVER //------------------------------------------------------------------------------ // EnterAccessControl ... ExitAccessControl //------------------------------------------------------------------------------ setp PRT_PROX_MAIN main_resume_label: //------------------------------------------------------------------------------ // Enter Access Control Section // Priority : PROX_MAIN // Resources: MotorA, MotorB //------------------------------------------------------------------------------ monal RES_AB, main_resume_label //------------------------------------------------------------------------------ // Drive around until through out of ACS //------------------------------------------------------------------------------ // Drive forward fwd OUTLIST_AB drive_loop_label: on OUTLIST_AB // Wait a random time between 3 and 9 seconds setv LV_DRIVE_TIME, SRC_RAN, FR_SEC_6 sumv LV_DRIVE_TIME, SRC_CON, FR_SEC_3 wait SRC_VAR, LV_DRIVE_TIME //------------------------------------------------------------------------------ // If LV_DRIVE_TIME is less than 6 seconds Then "turn left" Else "turn right" //------------------------------------------------------------------------------ chk SRC_CON, FR_SEC_6, LT, SRC_VAR, LV_DRIVE_TIME, elseif_drivetime_label off OUTLIST_A jmp endif_drivetime_label //------------------------------------------------------------------------------ elseif_drivetime_label: off OUTLIST_B //------------------------------------------------------------------------------ endif_drivetime_label: //------------------------------------------------------------------------------ // Turn for K_TURN_TIME //------------------------------------------------------------------------------ wait SRC_CON, K_TURN_TIME //------------------------------------------------------------------------------ // Jump back to loop start //------------------------------------------------------------------------------ jmp drive_loop_label //------------------------------------------------------------------------------ endt //============================================================================== //============================================================================== // // Task : TASK_PROX_SOUND // //------------------------------------------------------------------------------ // // Plays sounds depending on the ProxValue. // //============================================================================== task TASK_PROX_SOUND //------------------------------------------------------------------------------ // EnterAccessControl ... ExitAccessControl //------------------------------------------------------------------------------ setp PRT_PROX_SOUND sound_resume_label: //------------------------------------------------------------------------------ // Enter Access Control Section // Priority : PROX_SOUND // Resources: Sound //------------------------------------------------------------------------------ monal RES_S, sound_resume_label //------------------------------------------------------------------------------ // Forever play sounds according to GV_PROX_VALUE //------------------------------------------------------------------------------ sound_loop_label: //------------------------------------------------------------------------------ setv LV_SOUND_FRQ, SRC_VAR, GV_PROX_VALUE mulv LV_SOUND_FRQ, SRC_CON, K_FRQ_SCALER playv LV_SOUND_FRQ, K_SOUND_DURATION wait SRC_CON, K_SOUND_REPEAT_TIME //------------------------------------------------------------------------------ // Jump back to loop start //------------------------------------------------------------------------------ jmp sound_loop_label //------------------------------------------------------------------------------ endt //============================================================================== //============================================================================== // // Task : TASK_PROX_REACT // //------------------------------------------------------------------------------ // // Reacts on the ProxEvent. // //============================================================================== task TASK_PROX_REACT //------------------------------------------------------------------------------ // Set the task priority and select bug soundset //------------------------------------------------------------------------------ setp PRT_PROX_REACT sndset SNDSET_BUG //------------------------------------------------------------------------------ // Forever monitor the Prox event //------------------------------------------------------------------------------ react_loop_label: //------------------------------------------------------------------------------ // Wait for the Prox event //------------------------------------------------------------------------------ mone SRC_CON, EVENT_PROX, proxevent_label //------------------------------------------------------------------------------ wait_label: jmp wait_label //------------------------------------------------------------------------------ proxevent_label: //------------------------------------------------------------------------------ // EnterAccessControl ... ExitAccessControl //------------------------------------------------------------------------------ react_resume_label: //------------------------------------------------------------------------------ // Enter Access Control Section // Priority : PROX_REACT // Resources: MotorA, MotorB, Sound, VLL //------------------------------------------------------------------------------ monal RES_ABSV, react_resume_label //------------------------------------------------------------------------------ plays SND_SPECIAL1 setv LV_PAR1, SRC_CON, PAR_MOV_SHAKE setv LV_PAR2, SRC_CON, PAR_SHAKE_TIME calls SUB_MOVEMENTS //------------------------------------------------------------------------------ // Randomly select avoiding left or right //------------------------------------------------------------------------------ setv LV_AVOID_SELECT, SRC_RAN, 100 //------------------------------------------------------------------------------ // If LV_AVOID_SEL < 50 Then "Avoid left" Else "Avoid right" EndIf //------------------------------------------------------------------------------ chk SRC_CON, 50, GT, SRC_VAR, LV_AVOID_SELECT, elseif_avoid_label setv LV_PAR1, SRC_CON, PAR_AVOID_LEFT jmp endif_avoid_label //------------------------------------------------------------------------------ elseif_avoid_label: setv LV_PAR1, SRC_CON, PAR_AVOID_RIGHT //------------------------------------------------------------------------------ endif_avoid_label: //------------------------------------------------------------------------------ setv LV_PAR2, SRC_CON, PAR_AVOID_TIME calls SUB_AVOID //------------------------------------------------------------------------------ // Exit Access Control Section //------------------------------------------------------------------------------ monax //------------------------------------------------------------------------------ // Jump back to loop start //------------------------------------------------------------------------------ jmp react_loop_label //------------------------------------------------------------------------------ endt //============================================================================== //============================================================================== // // Task : TASK_PROX_DRIVER // //------------------------------------------------------------------------------ // // Generates the ProxValue/State and throughs the ProxEvent if the ProxLimit is // exceeded. // //============================================================================== task TASK_PROX_DRIVER //------------------------------------------------------------------------------ // Initialise //------------------------------------------------------------------------------ setv GV_PROX_VALUE, SRC_CON, 0 setv GV_PROX_STATE, SRC_CON, K_PROX_STATE_OFF //------------------------------------------------------------------------------ // Forever generate the Prox value //------------------------------------------------------------------------------ driver_foreverloop_label: //------------------------------------------------------------------------------ setv LV_LIGHT_DIF, SRC_CON, 0 //------------------------------------------------------------------------------ // Loop 4 times ... EndLoop //------------------------------------------------------------------------------ setv LV_LOOP_COUNTER, SRC_CON, 4 //------------------------------------------------------------------------------ start_proxloop_label: decvjn LV_LOOP_COUNTER, end_proxloop_label //------------------------------------------------------------------------------ wait SRC_CON, K_PROXLOOP_DELAY setv LV_LIGHT_BEFORE, SRC_SENRAW, SEN_LIGHT msg SRC_CON, K_PROX_MESSAGE wait SRC_CON, K_MEASURE_DELAY setv LV_LIGHT_AFTER, SRC_SENRAW, SEN_LIGHT subv LV_LIGHT_BEFORE, SRC_VAR, LV_LIGHT_AFTER //------------------------------------------------------------------------------ // If LV_LIGHT_BEFORE > LV_LIGHT_DIF Then "Update LV_LIGHT_DIF" EndIf //------------------------------------------------------------------------------ chk SRC_VAR, LV_LIGHT_BEFORE, GT, SRC_VAR, LV_LIGHT_DIF, endif_lbefore_label setv LV_LIGHT_DIF, SRC_VAR, LV_LIGHT_BEFORE //------------------------------------------------------------------------------ endif_lbefore_label: //------------------------------------------------------------------------------ // Jump back to prox loop start //------------------------------------------------------------------------------ jmp start_proxloop_label //------------------------------------------------------------------------------ end_proxloop_label: //------------------------------------------------------------------------------ // Update the global GV_PROX_VALUE //------------------------------------------------------------------------------ setv GV_PROX_VALUE, SRC_VAR, LV_LIGHT_DIF //------------------------------------------------------------------------------ // Update the global GV_PROX_STATE and possibly through the Prox event //------------------------------------------------------------------------------ // If GV_PROX_VALUE > K_PROX_LIMIT Then ... EndIf //------------------------------------------------------------------------------ chk SRC_CON, K_PROX_LIMIT, LT, SRC_VAR, GV_PROX_VALUE, endif_proxvalue1_label //------------------------------------------------------------------------------ // If GV_PROX_STATE = OFF Then ... EndIf //------------------------------------------------------------------------------ chk SRC_CON, K_PROX_STATE_OFF, EQ, SRC_VAR, GV_PROX_STATE, endif_proxstate1_label setv GV_PROX_STATE, SRC_CON, K_PROX_STATE_ON event EVENT_PROX //------------------------------------------------------------------------------ endif_proxstate1_label: //------------------------------------------------------------------------------ endif_proxvalue1_label: //------------------------------------------------------------------------------ // If GV_PROX_VALUE < K_PROX_LIMIT_HYST Then ... EndIf //------------------------------------------------------------------------------ chk SRC_CON, K_PROX_LIMIT_HYST, GT, SRC_VAR, GV_PROX_VALUE, endif_proxvalue2_label //------------------------------------------------------------------------------ // If GV_PROX_STATE = ON Then ... EndIf //------------------------------------------------------------------------------ chk SRC_CON, K_PROX_STATE_ON, EQ, SRC_VAR, GV_PROX_STATE, endif_proxstate2_label setv GV_PROX_STATE, SRC_CON, K_PROX_STATE_OFF //------------------------------------------------------------------------------ endif_proxstate2_label: //------------------------------------------------------------------------------ endif_proxvalue2_label: //------------------------------------------------------------------------------ // Jump back to forever loop start //------------------------------------------------------------------------------ jmp driver_foreverloop_label //------------------------------------------------------------------------------ endt //==============================================================================