ref: 82b046f36f8084a22bbb5d71edd0edd9179561eb
dir: /appl/cmd/lego/clockface.b/
# Model 1
implement Clockface;
include "sys.m";
sys: Sys;
include "draw.m";
Clockface: module {
init: fn(ctxt: ref Draw->Context, argv: list of string);
};
hmpath: con "motor/0"; # hour-hand motor
mmpath: con "motor/2"; # minute-hand motor
allmpath: con "motor/012"; # all motors (for stopall msg)
hbpath: con "sensor/0"; # hour-hand sensor
mbpath: con "sensor/2"; # minute-hand sensor
lspath: con "sensor/1"; # light sensor;
ONTHRESH: con 780; # light sensor thresholds
OFFTHRESH: con 740;
NCLICKS: con 120;
MINCLICKS: con 2; # min number of clicks required to stop a motor
Hand: adt {
motor: ref Sys->FD;
sensor: ref Sys->FD;
fwd: array of byte;
rev: array of byte;
stop: array of byte;
pos: int;
time: int;
};
lightsensor: ref Sys->FD;
allmotors: ref Sys->FD;
hourhand: ref Hand;
minutehand: ref Hand;
timedata: array of byte;
readq: list of Sys->Rread;
verbose := 0;
init(nil: ref Draw->Context, argv: list of string)
{
sys = load Sys Sys->PATH;
argv = tl argv;
if (len argv > 0 && hd argv == "-v") {
verbose++;
argv = tl argv;
}
if (len argv != 1) {
sys->print("usage: [-v] legodir\n");
raise "fail:usage";
}
legodir := hd argv + "/";
# set up our control file
f2c := sys->file2chan("/chan", "clockface");
if (f2c == nil) {
sys->print("cannot create clockface channel: %r\n");
return;
}
# get the motor files
log("opening motor files");
hm := sys->open(legodir + hmpath, Sys->OWRITE);
mm := sys->open(legodir +mmpath, Sys->OWRITE);
allmotors = sys->open(legodir + allmpath, Sys->OWRITE);
if (hm == nil || mm == nil || allmotors == nil) {
sys->print("cannot open motor files\n");
raise "fail:error";
}
# get the sensor files
log("opening sensor files");
hb := sys->open(legodir + hbpath, Sys->ORDWR);
mb := sys->open(legodir + mbpath, Sys->ORDWR);
lightsensor = sys->open(legodir + lspath, Sys->ORDWR);
if (hb == nil || mb == nil) {
sys->print("cannot open sensor files\n");
raise "fail:error";
}
hourhand = ref Hand(hm, hb, array of byte "r7", array of byte "f7", array of byte "s7", 0, 00);
minutehand = ref Hand(mm, mb, array of byte "f7", array of byte "r7", array of byte "s7", 0, 00);
log("setting sensor types");
setsensortypes(hourhand, minutehand, lightsensor);
# get the hands to 12 o'clock
reset();
log(sys->sprint("H %d, M %d", hourhand.pos, minutehand.pos));
spawn srvlink(f2c);
}
srvlink(f2c: ref Sys->FileIO)
{
tick := chan of int;
spawn eggtimer(tick);
for (;;) alt {
(nil, count, fid, rc) := <-f2c.read =>
if (rc == nil) {
close(fid);
continue;
}
if (count < len timedata) {
rc <-= (nil, "read too small");
continue;
}
if (open(fid))
readq = rc :: readq;
else
rc <-= (timedata, nil);
(nil, data, fid, wc) := <-f2c.write =>
if (wc == nil) {
close(fid);
continue;
}
(nil, toks) := sys->tokenize(string data, ": \t\n");
if (len toks == 2) {
wc <-= (len data, nil);
hourhand.time = int hd toks % 12;
minutehand.time = int hd tl toks % 60;
sethands();
} else if (len toks == 1 && hd toks == "reset") {
wc <-= (len data, nil);
reset();
} else
wc <-= (0, "syntax is hh:mm or `reset'");
<-tick =>
if (++minutehand.time == 60) {
minutehand.time = 0;
hourhand.time++;
hourhand.time %= 12;
}
sethands();
}
}
readers: list of int;
open(fid: int): int
{
for (rlist := readers; rlist != nil; rlist = tl rlist)
if (hd rlist == fid)
return 1;
readers = fid :: readers;
return 0;
}
close(fid: int)
{
rlist: list of int;
for (; readers != nil; readers = tl readers)
if (hd readers != fid)
rlist = hd readers :: rlist;
readers = rlist;
}
eggtimer(tick: chan of int)
{
next := sys->millisec();
for (;;) {
next += 60*1000;
sys->sleep(next - sys->millisec());
tick <-= 1;
}
}
clicks(): (int, int)
{
h := hourhand.time;
m := minutehand.time;
h = ((h * NCLICKS) / 12) + ((m * NCLICKS) / (12 * 60));
m = (m * NCLICKS) / 60;
return (h, m);
}
sethands()
{
timedata = array of byte sys->sprint("%2d:%.2d\n", (hourhand.time+11) % 12 + 1, minutehand.time);
for (; readq != nil; readq = tl readq)
alt {
(hd readq) <-= (timedata, nil) => ;
* => ;
}
(hclk, mclk) := clicks();
for (i := 0; i < 6; i++) {
hdelta := clickdistance(hourhand.pos, hclk, NCLICKS);
mdelta := clickdistance(minutehand.pos, mclk, NCLICKS);
if (hdelta != 0)
sethand(hourhand, hdelta);
else if (mdelta != 0)
sethand(minutehand, mdelta);
else
break;
}
releaseall();
}
clickdistance(start, stop, mod: int): int
{
if (start > stop)
stop += mod;
d := (stop - start) % mod;
if (d > mod/2)
d -= mod;
return d;
}
setsensortypes(h1, h2: ref Hand, ls: ref Sys->FD)
{
button := array of byte "b0";
light := array of byte "l0";
sys->write(h1.sensor, button, len button);
sys->write(h2.sensor, button, len button);
sys->write(ls, light, len light);
}
HOUR_ADJUST: con 1;
MINUTE_ADJUST: con 2;
reset()
{
# run the motors until hands are well away from 12 o'clock (below threshold)
val := readsensor(lightsensor);
if (val > OFFTHRESH) {
triggered := chan of int;
log("wait for hands clear of light sensor");
spawn lightwait(triggered, lightsensor, 0);
forward(minutehand);
reverse(hourhand);
val = <-triggered;
stopall();
log("sensor "+string val);
}
resethand(hourhand);
hourhand.pos += HOUR_ADJUST;
resethand(minutehand);
minutehand.pos += MINUTE_ADJUST;
sethands();
}
sethand(hand: ref Hand, delta: int)
{
triggered := chan of int;
dir := 1;
if (delta < 0) {
dir = -1;
delta = -delta;
}
if (delta > MINCLICKS) {
spawn handwait(triggered, hand, delta - MINCLICKS);
if (dir > 0)
forward(hand);
else
reverse(hand);
<-triggered;
stop(hand);
hand.pos += dir * readsensor(hand.sensor);
} else {
startval := readsensor(hand.sensor);
if (dir > 0)
forward(hand);
else
reverse(hand);
stop(hand);
hand.pos += dir * (readsensor(hand.sensor) - startval);
}
if (hand.pos < 0)
hand.pos += NCLICKS;
hand.pos %= NCLICKS;
}
resethand(hand: ref Hand)
{
triggered := chan of int;
val: int;
# run the hand until the light sensor is above threshold
log("running hand until light sensor activated");
spawn lightwait(triggered, lightsensor, 1);
forward(hand);
val = <-triggered;
stop(hand);
log("sensor "+string val);
startclick := readsensor(hand.sensor);
# advance until light sensor drops below threshold
log("running hand until light sensor clear");
spawn lightwait(triggered, lightsensor, 0);
forward(hand);
val = <-triggered;
stop(hand);
log("sensor "+string val);
stopclick := readsensor(hand.sensor);
nclicks := stopclick - startclick;
log(sys->sprint("startpos %d, endpos %d (nclicks %d)", startclick, stopclick, nclicks));
hand.pos = nclicks/2;
}
stop(hand: ref Hand)
{
sys->seek(hand.motor, big 0, Sys->SEEKSTART);
sys->write(hand.motor, hand.stop, len hand.stop);
}
stopall()
{
msg := array of byte "s0s0s0";
sys->seek(allmotors, big 0, Sys->SEEKSTART);
sys->write(allmotors, msg, len msg);
}
releaseall()
{
msg := array of byte "F0F0F0";
sys->seek(allmotors, big 0, Sys->SEEKSTART);
sys->write(allmotors, msg, len msg);
}
forward(hand: ref Hand)
{
sys->seek(hand.motor, big 0, Sys->SEEKSTART);
sys->write(hand.motor, hand.fwd, len hand.fwd);
}
reverse(hand: ref Hand)
{
sys->seek(hand.motor, big 0, Sys->SEEKSTART);
sys->write(hand.motor, hand.rev, len hand.rev);
}
readsensor(fd: ref Sys->FD): int
{
buf := array[4] of byte;
sys->seek(fd, big 0, Sys->SEEKSTART);
n := sys->read(fd, buf, len buf);
if (n <= 0)
return -1;
return int string buf[:n];
}
handwait(reply: chan of int, hand: ref Hand, clicks: int)
{
blk := array of byte ("b" + string clicks);
log("handwait "+string blk);
sys->seek(hand.sensor, big 0, Sys->SEEKSTART);
if (sys->write(hand.sensor, blk, len blk) != len blk)
sys->print("handwait write error: %r\n");
reply <-= readsensor(hand.sensor);
}
lightwait(reply: chan of int, fd: ref Sys->FD, on: int)
{
thresh := "";
if (on)
thresh = "l>" + string ONTHRESH;
else
thresh = "l<" + string OFFTHRESH;
blk := array of byte thresh;
log("lightwait "+string blk);
sys->seek(fd, big 0, Sys->SEEKSTART);
sys->write(fd, blk, len blk);
reply <-= readsensor(fd);
}
log(msg: string)
{
if (verbose)
sys->print("%s\n", msg);
}