Elevator

See YT Video (German language with English subtitles)

File linit.c

// I/O ports
const LIFT   = 0;
const TOWER  = 1;
const DOORS  = 8;  // base address

const NUM_FLOORS = 3;

var heights[] = {4, 5};

File lift.c

import "lib/techage.c"
import "sys/stdlib.asm"
import "sys/stdio.asm"
import "sys/math.asm"
import "linit.c"

const COLOR = 2;
var IO_RED   = "\003";
var IO_AMBER = "\002";
var IO_GREEN = "\001";

var route[] = {0,0,0};
var last_floor = 0;
var timeout = 0;
var NUM_FLOOR_CTLR = NUM_FLOORS * 2;

func init() {
  var i;
  var port;

  send_cmnd(TOWER, COLOR, IO_AMBER);

  // close all doors
  for(i = 0; i < NUM_FLOOR_CTLR; i++) {
    port = DOORS + i;
    send_cmnd(port, 11, "\002");
  }

  // reset lift
  send_cmnd(LIFT, 19, "");
  sleep(30);
  send_cmnd(TOWER, COLOR, IO_GREEN);
}

func calc_height(dest_floor) {
  var i;
  var sum = 0;

  if(dest_floor > last_floor) {
    for(i = last_floor; i < dest_floor; i++) {
      sum = sum + heights[i];
    }
    return sum;
  } else {
    for(i = last_floor; i > dest_floor; i--) {
      sum = sum + heights[i - 1];
    }
    return -sum;
  }
}

func open_doors(floor) {
  var port;

  send_cmnd(TOWER, COLOR, IO_RED);
  port = DOORS + floor * 2;
  send_cmnd(port + 0, 11, "\001");
  send_cmnd(port + 1, 11, "\001");
  sleep(10);
  send_cmnd(TOWER, COLOR, IO_GREEN);
  last_floor = floor;
  timeout = 30;
}

func close_doors(floor) {
  var port;

  send_cmnd(TOWER, COLOR, IO_RED);
  port = DOORS + floor * 2;
  send_cmnd(port + 0, 11, "\002");
  send_cmnd(port + 1, 11, "\002");
  sleep(10);
  send_cmnd(TOWER, COLOR, IO_GREEN);
}

func move_lift(floor) {
  var h;

  send_cmnd(TOWER, COLOR, IO_RED);
  h = calc_height(floor);
  route[1] = h;
  send_cmnd(LIFT, 18, route);
  sleep(20 + abs(h));
  last_floor = floor;
  send_cmnd(TOWER, COLOR, IO_GREEN);
}

func loop() {
  var val;
  var port;

  port = get_next_inp_port();
  if(port != 0xffff) {
    val = input(port);
    if(val < 10) {
      if(val != last_floor) {
        move_lift(val);
      }
      open_doors(val);
    } else if(val < 20) {
      close_doors(last_floor);
      move_lift(val - 10);
      open_doors(val - 10);
    }
  }
  if(timeout > 0) {
    timeout--;
    if(timeout == 0) {
      close_doors(last_floor);
    }
  }
}