fasdf

Forrit:

// z grows as it drives forward, y>>0 is wincon (platform), edges are x=+-width/2
let origin  = [0,0,0]
let initVel = [0,0,0]

let location     = [x, y, z] // z is forward, y is vertical, x is width
let velocity     = [Vx, Vy, Vz]
let direction    = [Dx, Dy] // coupled to velocity is not needed for high logic
let acceleration = [Ax, Ay, Az]

def length:
  sqrt(x*x+y*y+z*z)


def loop:
  case ($LOCATION, $DIRECTION) of
    stone? -> getLost // may not be needed
    isLost -> courseAdjustment
    button -> Stop >>= releaseTrigger >>= Continue
    funnel -> Stop >>= pourWater >>= Continue
    wincon -> Stop;
    _ -> Continue

  update >>= loop

stone?
  makes the robot turn to try and avoid the big stone. This function would override isLost until courseAdjustment would be needed again.

getLost
  Turn left basically, also makes sure not to turn too much.

Stop
  Stops the car for $arg mseconds or untill instructed to Continue

isLost
  Condition: Direction is not parallell to z (tolerates some discrepancy bounded by deltaX and deltaY from where discrepancy started).

courseAdjustment
  Makes the car drive parallel to z (straight forward).

button
  The location where the stop command should be sent so that it hits the button

releaseTrigger
  Release trigger
  suspend(time) // Wait for Water

funnel
  The location where the stop command should be sent so the water lands in the funnel.

pourWater
  moveStraw
  releaseWater.
  suspend(time) // Wait for Water

wincon
  When direction is perpendicular to y and the robot is high enough e.g.
    (Vy = 0) and (platform >= y > stone)