' the speed of the robot motors (a constant)
SPEED = 30

' grab what state we are in using the 
' getvariable routine
state = GetVariable("state")

' jump to the appropriate state
select case state
  case 0:
    SetVariable "Status: ", "Opening grippers"
  
    ' start the open gripper timer if it is not already
    ' initialized
    if GetVariable("GRABBER_OPEN") = "0" then
      SetVariable "GRABBER_OPEN", Timer()
    end if

    ' only execute this step for < 2 seconds
    if Timer() - GetVariable("GRABBER_OPEN") < 3 then
      ' set the grabber variable to open ... this is 
      ' passed to the NXT as a motor command
      SetVariable "GRABBER", 180
    else  
      ' set the grabber variable to neutral
      SetVariable "GRABBER", 128
      ' re-zero the time so we can use it again below
      SetVariable "GRABBER_OPEN", 0
      ' transition to the next state
      SetVariable "state", 1
    end if

  case 1:
    SetVariable "Status: ", "Searching for ball"

    ' grab the COG size that is created by the COG 
    ' module
    size = GetVariable("COG_BOX_SIZE")
    xw = (GetVariable("IMAGE_WIDTH")/2)
    ' if a big enough blob is seen then proceed to 
    ' the next state
    if size > 20 and GetVariable("COG_X") > xw - 60 and GetVariable("COG_X") < xw + 60  then
			SetVariable "MAX_SIZE", size	
      SetVariable "state", 2
    else
      ' otherwise turn left until we see something big 
      ' enough to track the left and right motor 
      ' variables are passed to the NXT
      SetVariable "LEFT_MOTOR", 128
      SetVariable "RIGHT_MOTOR", 215
    end if
  case 2:
    SetVariable "Status: ", "Approaching ball"
    
    size = GetVariable("COG_BOX_SIZE")

    maxSize = GetVariable("MAX_SIZE")

		if size > maxSize then
			SetVariable "MAX_SIZE", size	
		end if
		
    ' if we passed over the ball we are near enough to 
    ' the ball to grab it this is determined by the COG 
    ' going blank OR the ball suddenly got much smaller 
    ' since we probably picked up a bit of blue noise 
    ' once the ball when out of view
    if (size < 20 or GetVariable("COG_Y") = "") and maxSize > 100 then

      ' stop the motors
      SetVariable "LEFT_MOTOR", 128
      SetVariable "RIGHT_MOTOR", 128
      ' turn on the grabber
      SetVariable "GRABBER", 40
      ' continue to the next state
      SetVariable "state", 3
    else

      ' otherwise keep steering towards the ball's COG
      
      ' get the screen with and height (will always 
      ' be 320 in our case)
      xw = (GetVariable("IMAGE_WIDTH")/2)

      ' how far off center is the blob?
      xMove=GetVariable("COG_X")-xw
      xoff = xw - 5

      ' if the blob is off to the right then increase 
      ' the right motor a little depending on how far 
      ' off right we are
      if xMove > 0 then

        rightMove = 215-SPEED+(((xoff-xMove-k)/xoff)*SPEED)
        leftMove = 215

      else

        ' otherwise up the left motor in a similar manner
        leftMove = 215-SPEED+(((xoff-(-xMove)-k)/xoff)*SPEED)
        rightMove = 215

      end if

      ' set the variables so that the NXT module can use them
      SetVariable "LEFT_MOTOR", leftMove
      SetVariable "RIGHT_MOTOR", rightMove

    end if

  case 3:
    ' we've got a ball now head towards the red cone!!  
    SetVariable "Status: ", "Searching for cone"
    
    ' the processing pipeline has already switched the blob 
    ' tracking to a red object since state >= 3. We just 
    ' need to find it and head towards it.
    
    size = GetVariable("COG_BOX_SIZE")
    if size > 30 then
      SetVariable "state", 4
    else
      ' turn left until we see something big enough to track
      SetVariable "LEFT_MOTOR", 128
      SetVariable "RIGHT_MOTOR", 215
    end if
  case 4:
    SetVariable "Status: ", "Approaching cone"
    
    ' this routine is exactly like the one above 
    ' except for the criteria that the blob cog 
    ' size is 200
    
    size = GetVariable("COG_BOX_SIZE")
    
    ' if we are near enough to a cone drop the ball
    if size > 200 then
      ' stop the robot
      SetVariable "LEFT_MOTOR", 128
      SetVariable "RIGHT_MOTOR", 128
      SetVariable "state", 5
    else

      ' otherwise keep steering towards the cone's COG
      xw = (GetVariable("IMAGE_WIDTH")/2)

      xMove=GetVariable("COG_X")-xw

      xoff = xw - 5

      if xMove > 0 then

        rightMove = 215-SPEED+(((xoff-xMove-k)/xoff)*SPEED)
        leftMove = 215

      else

        leftMove = 215-SPEED+(((xoff-(-xMove)-k)/xoff)*SPEED)
        rightMove = 215

      end if

      SetVariable "LEFT_MOTOR", leftMove
      SetVariable "RIGHT_MOTOR", rightMove

    end if
  case 5:
  
    SetVariable "Status: ", "Opening grippers"
    
    ' initialize timer
    if GetVariable("GRABBER_OPEN") = "0" then
      SetVariable "GRABBER_OPEN", Timer()
    end if

    ' open grabber .. but only execute this step 
    ' for < 3 seconds
    if Timer() - GetVariable("GRABBER_OPEN") < 3 then
      SetVariable "GRABBER", 180
    else
      SetVariable "GRABBER", 128
      SetVariable "GRABBER_OPEN", "0"
      SetVariable "state", 6
    end if

  case 6:
    SetVariable "Status: ", "Misson Complete"

    ' clean up for next run and stop all robot motors
    SetVariable "GRABBER", 128
    SetVariable "GRABBER_OPEN", "0"
    SetVariable "GRABBER_CLOSE", "0"
    SetVariable "LEFT_MOTOR", 128
    SetVariable "RIGHT_MOTOR", 128
end select