;This is a program to control a model train on a simple rail network. If
;you have visited our exhibition stand you may have seen this application
;working.
;
;The track is shaped like this:
;
;

; Points
;A =========//========== B
; //
; //
; //
;C =====//============== D
; Points

;
;
;The train's power is controlled via a PulseWidthOut object that
;pulse-width-modulates the 12V power supply. The points are controlled by
;a MOSFET digital output card. The only sensors are four microswitches -
;one at each station: A, B, C, D, that detect the train hitting the
;buffers.
;
;The program allows the operator to key in a series of destinations on
;the keypad. It then controls the train's power and the two sets of
;points to move the train from station to station, and reflects the
;train's position on a Graphic LCD display.
;
;The train cannot be run into the buffers at high speed else it derails.
;However it is wasteful to run it so slowly the whole way along the
;track. So a continual optimisation routine keeps the train running fast
;along the track, reducing the power just before it hits the buffers.
;There is no way to tell where the train is on the track - the
;optimisation works by slowly increasing the time at full power and then
;timing how long it spends at low power before arriving.
;
;Control of the routing via the points is done using 'recursion'. This
;is to demonstrate using this technique - it is not the only way to solve
;the problem. The routing algorithm then reduces to an ARRAY of primitive
;moves and a simple procedure to process it.




;Some constants to define positions and moves on the track...
;They are used in an array to work out how
; to move from one point to any other...
#DEFINE STATION_A 0
#DEFINE STATION_B 1
#DEFINE STATION_C 2
#DEFINE STATION_D 3
#DEFINE NO_MOVE 10 ;For when we've been requested to move to where we are now!
#DEFINE DIRECT 11 ;For those cases where we just move straight down the track.
#DEFINE VIA_POINTS 12 ;For those cases where we go via the points.

#DEFINE START_WIDTH 350
#DEFINE PWM_PERIOD 1000

;Create all the objects to control the I/O of the train system.
TO init
  MAKE pwm PulseWidthOut (3,PWM_PERIOD,START_WIDTH,1) ;PWM control of train motor.
  pwm.On ;power on
  
  MAKE motor_fwd Digital(4) ;Energise train in forward direction
  MAKE motor_rvs Digital(5) ;Energise train in reverse direction
  
  MAKE points_1_turn Digital(128) ;Control the points solenoids.
  MAKE points_1_strt Digital(129) ;[Note: must leave 200mS between each use]
  MAKE points_2_turn Digital(130) ; --"--
  MAKE points_2_strt Digital(131) ; --"--
  
  MAKE limit_switchs Digital(504,507) ;[all 4 limit switches as one digital port]

  MAKE kpd Keypad (0,496)
  MAKE keybuff Buffer("") ;To Hold a FIFO of the key presses.
  
  ;Initialise values of some global variables.
  current_station := STATION_A ;assume the train is here to start with.
  forward := TRUE
  
  ;Set up the graphics window etc.
  MAKE g GraphicsLCD(4)
  w := g.Window(0 0 239 127 0)
  g.Sprite(1):=train_sprite.Address
  global_position := -1
  draw_main_screen
  g.Update
  
  ;Set up the speed profiling matrix for getting the train
  ;to it's destination quickly, and slowing down before the
  ;buffers.
  speed_prof_array := NEW Array
  ( 8,16
     0, 7, 0, 0,
     5, 0, 10, 0,
     0, 12, 0, 0,
     0, 0, 0, 0
  )
  
END

;The main procedure - get a move-request and then do it.
TO main
  START read_keypad ; New Task for reading the keypad
  START do_graphics ; New Task for displaying the graphics
  
  FOREVER ;Foreground Task for controlling the train
  [
    IF keybuff.Queue ;Read a key out of the buffer and act on it...
    [
      shuttle (keybuff.Get - 'A');Navigate the train accross the network...
      keybuff.Flush
      print_keylist
      WAIT 1000 ; Stop for this long at each destination.
    ]
  ]
END


;This array specifies how to get to each point from each other point,
; using intermediate steps if necessary.
;Where a station is listed as the move, it means "go to this staion and try again".
Array move_matrix(8 , 16)
  NO_MOVE , DIRECT , STATION_B , STATION_B , ;From A to A,B,C,D
  DIRECT , NO_MOVE , VIA_POINTS , STATION_C , ;From B to A,B,C,D
  STATION_B , VIA_POINTS , NO_MOVE , DIRECT , ;From C to A,B,C,D
  STATION_C , STATION_C , DIRECT , NO_MOVE ;From D to A,B,C,D
END

;move from the current station to any other station.
TO shuttle(destination)

  ;Find what kind of move to make by looking it up
  ; from the ARRAY using current_position and destination:
  move_to := move_matrix.(current_station * 4 + destination)
  
  curr := current_station ;These two signal speed profile task which route we are on.
  dest := destination
  
  ;do the move...
  SELECT CASE move_to
    CASE NO_MOVE
    [
                        ;Do nothing.
    ]
    CASE DIRECT
    [
      set_points(FALSE) ;set the points to straight ahead...
      move_train ;then move train.
    ]
    CASE VIA_POINTS
    [
      set_points(TRUE) ;set the points to turn off...
      move_train ;then move train.
    ]
    CASE ELSE ;we have an intermediate station to go through first...
    [
      shuttle(move_to) ;call this routine recursively to go to intermediate station.
      shuttle(destination) ;call this routine recursively - to go to final destination..?
    ]
END

;Operate both sets of points. Parameter 'turn': redirect train, or go straight through.
;Note: the points take too much current to be powered directly - they are fed pulses of
;current from a large capacitor, hence the 200mS wait in this procedure to allow the cap. to
;recharge.
TO set_points(turn)
  IF turn
  [
    points_1_turn.On
    WAIT 20
    points_1_turn.Off
    WAIT 200
    points_2_turn.On
    WAIT 20
    points_2_turn.Off
  ]
  ELSE
  [
    points_1_strt.On
    WAIT 20
    points_1_strt.Off
    WAIT 200
    points_2_strt.On
    WAIT 20
    points_2_strt.Off
  ]
END


;Operate the train motor to move it until it reaches the next limit switch.
; Change the direction so the next move is correct.
TO move_train
  LOCAL motor_dig
  
  IF forward ;Train motor controller: which direction do we energise?
     motor_dig := motor_fwd
  ELSE
     motor_dig := motor_rvs
    
  motor_running := TRUE ;initialise signal to profiling task.
  START [profile_speed_task] ;start speed profiling task.
  
  motor_dig.On ; Power on
  await_no_limit ; make sure we have left the current station...
  await_a_limit ; ...before we detect the next one.
  motor_dig.Off ; Power off when we hit the buffers.
  
  motor_running := FALSE ;signal to speed profile task that move is done.
  AWAIT motor_running ;wait till speed profile task has synchronised.
  
  forward := NOT forward ;change the direction for the next run.
END


;Runs along-side the main motor on/off task 'TO move_train'
;and ramps the PWM power control up and down to produce
;a smooth and timely acceleration and deceleration before
;running into the buffers.
;The time spent at cruising speed is adjusted depending on
;whether the train hit the buffers before or after the speed
;profiling had finished.

TO profile_speed_task
  ;find the current estimate for cruise time.
  LOCAL cruise_time := speed_prof_array.(curr * 4 + dest)
  
  ;ramp up
  pwm.Width := START_WIDTH
  EVERY 10
  [
     pwm.Width := pwm.Width + 20
     IF pwm.Width >= PWM_PERIOD
      BREAK
  ]
  
  ;cruise
  WAIT cruise_time * 100
  
  ;slow dowm
  EVERY 10
  [
     pwm.Width := pwm.Width - 10
     IF pwm.Width <= START_WIDTH
      BREAK
  ]
  pwm.Width := START_WIDTH
  
  ;dead band
  WAIT 1000

  ;adjust the cruise time.
  IF motor_running ; If the motor hasn't been turned off by the time we get here
  [ ; then we were going too slow.
    cruise_time := cruise_time + 1
  ]
  ELSE
  [
    cruise_time := cruise_time - 1
  ]
  
  ;set the new estimate for cruise time.
  speed_prof_array.(curr * 4 + dest) := cruise_time
  
  AWAIT NOT motor_running ;synchronise with main task.
  motor_running := TRUE ;signal that this task is done.
END

;Wait until a limit switch is activated, and record which one it was.
TO await_a_limit
  LOCAL a
  DO
  [
    a := limit_switchs.Value
    WAIT 10
  ] UNTIL a <> $F
  current_station := station_from_limit(a EOR $F)
END

;Wait until no limit switch is activated
TO await_no_limit
  AWAIT limit_switchs.Value = $F
END

;Convert a limit switch number to a station number.
TO station_from_limit(limit_bit)
  SELECT CASE limit_bit
    CASE 1 RETURN 0
    CASE 2 RETURN 1
    CASE 4 RETURN 2
    CASE 8 RETURN 3
    CASE ELSE
    [
      pwm.Off
      PRINT "bad station!",CR
    ]
    RETURN -1
END

;A table to convert key numbers to meaningful symbols.
Array key_table(8 , 16) ;| Key order | Key legend
'*', 0, '#', 'D', ;| 12 13 14 15 | 7 8 9 A
1, 2, 3, 'C', ;| 8 9 10 11 | 4 5 6 B
4, 5, 6, 'B', ;| 4 5 6 7 | 1 2 3 C
7, 8, 9, 'A' ;| 0 1 2 3 | * 0 # .
END

;Read the keypad and put keys into a FIFO buffer.
TO read_keypad
  LOCAL k
  FOREVER
  [
    DO
    [
      k := key_table.(kpd.Get) ;read a key from the keypad.
      IF k = '*' empty_keybuffer
    ] UNTIL k >= 'A'
    keybuff.Put(k)
    print_keylist
  ]
END


TO empty_keybuffer
  keybuff.Lock
  WHILE keybuff.Queue
  [
    keybuff.Get
    keybuff.Flush
    print_keylist
  ]
  keybuff.UnLock
END

;************* THE GRAPHICS *****************

;Some constants that define the shape and size of the track.
#DEFINE TRACKW 2
#DEFINE TRACKSPC 30
#DEFINE TRACKY1 40
#DEFINE TRACKY2 (TRACKY1 + TRACKW)
#DEFINE TRACKY3 (TRACKY1 + TRACKSPC)
#DEFINE TRACKY4 (TRACKY3 + TRACKW)
#DEFINE TRACKX1 40
#DEFINE TRACKX2 180
#DEFINE TRACKMIDX (TRACKX1 + (TRACKX2 - TRACKX1) DIV 2)
#DEFINE TRACKX3 (TRACKMIDX - TRACKSPC DIV 2)
#DEFINE TRACKX4 (TRACKMIDX + TRACKSPC DIV 2)

;The highest level graphics routine.
; - read the current_station and put the sprite in the
; correct location.
TO do_graphics
  LOCAL old_station := current_station

  EVERY 100
  [
    IF current_station <> old_station
    [
      old_station := current_station
      redraw_train_sprite(current_station)
    ]
    display_optimised_routes ;!!! best place?
    g.Update ;Update the graphics regularly.
  ]
END


;Remove the sprite from it's current position, and redraw
; in the new position.
TO redraw_train_sprite(pos)
  draw_train_sprite(global_position)
  draw_train_sprite(pos)
END

;Plot the sprite in the position given.
TO draw_train_sprite(pos)
  global_position := pos
  SELECT CASE pos
  CASE 0
    w.Sprite(TRACKX1 , TRACKY4+13, 1)
  CASE 1
    w.Sprite(TRACKX2-16, TRACKY4+13, 1)
  CASE 2
    w.Sprite(TRACKX1 , TRACKY2+13, 1)
  CASE 3
    w.Sprite(TRACKX2-16, TRACKY2+13, 1)
END


;Display the route speed figures by each route.
TO display_optimised_routes
  PRINT TO w
    , LEFT
    , FONT 3
    , GOTOXY (TRACKX1+5, TRACKY3-9) , speed_prof_array.(4) :2
    , GOTOXY (TRACKX2-15, TRACKY3-9) , speed_prof_array.(1) :2
    , GOTOXY (TRACKX1+5, TRACKY1-9) , speed_prof_array.(14):2
    , GOTOXY (TRACKX2-15, TRACKY1-9) , speed_prof_array.(11):2
    , GOTOXY (TRACKX3,TRACKY1-9) , speed_prof_array.(6) :1
    , GOTOXY (TRACKX4,TRACKY3-9) , speed_prof_array.(9) :1
  
END


;Draw the main screen.
TO draw_main_screen
  w.Line(TRACKX1,TRACKY1,TRACKX2,TRACKY1)
  w.Line(TRACKX1,TRACKY2,TRACKX2,TRACKY2)
  w.Line(TRACKX1,TRACKY3,TRACKX2,TRACKY3)
  w.Line(TRACKX1,TRACKY4,TRACKX2,TRACKY4)
  w.Line(TRACKX3,TRACKY1,TRACKX4,TRACKY3)
  w.Line(TRACKX3-TRACKW,TRACKY1+TRACKW,TRACKX4-TRACKW,TRACKY3+TRACKW)

  PRINT TO w
    , FONT 1
    , CENTRE
    , "Micro-Robotics"
    , FONT 0
    , CR
    , "Example Application"
    ;, LEFT
    , FONT 1
    , GOTOXY (TRACKX1-9, TRACKY3), "A"
    , GOTOXY (TRACKX2+9, TRACKY3), "B"
    , GOTOXY (TRACKX1-9, TRACKY1), "C"
    , GOTOXY (TRACKX2+9, TRACKY1), "D"
END

;Print the list of stations on to the LCD.
TO print_keylist
  PRINT TO w
    , FONT 3
    , LEFT
    , GOTOXY (10,10)
    , keybuff
    , " "
    
    ;g.update
END

;The bitmap of the train.
Array train_sprite (8, 36)
16,13,0,0
%00000000 %00001111
%11111111 %00000110
%00111110 %00000110
%00111110 %00000110
%00111110 %00000110
%00111110 %00000110
%11111111 %11111111
%11111111 %11111111
%11111111 %11111111
%11111111 %11111111
%11111111 %11111111
%01111111 %10011110
%00110011 %00001100
END