\ SCAN2.TXT - PEAK FINDER FOR THE SEEKER
\
\ REQUIRES: DIFFMOTOR3.TXT LIGHT.TXT FILTER1.TXT SCAN1.TXT

 3 VALUE SCAN_PEAK_THRESH

: FIND_PEAK ( dir steps -- f )

  \ Now set the threshold as the difference between the
  \ average and the peak

  SCAN_CURRENT_PEAK SCAN_AVERAGE FILTER_GET
  - 2 / 1 + 4 MIN TO SCAN_PEAK_THRESH 

  0 ROT ROT ( 0 dir steps --)    \ Set up failure 

  0 DO
    \ Now figure out if we have found the peak or
    \ a brighter light
   
    1 SENSOR_LIGHT_GET

    SCAN_CURRENT_PEAK SCAN_PEAK_THRESH - >

    \ If the current reading is bigger than the peak
    \ minus the thresh, we have a new light to follow.

    IF   SWAP 1+ SWAP LEAVE
    THEN

    \ We have not found it yet, stop and spin the bot...

    DUP IF BOT_RIGHT ELSE BOT_LEFT THEN

    SCAN_TURN_DELAY 0 timer_SET

    BEGIN
      0 timer_GET 0=
    UNTIL
    BOT_STOP

  LOOP

  DROP ;
  
