next up previous
Next: A.28 vc-mouse.setl Up: A. WEBeye Source Code Previous: A.26 vc-master.cgi

  
A.27 vc-model.setl

Called by parent program:
vc-do.setl     (Section A.11 [vc-do.setl])

Calls child program:
vc-seq.setl     (Section A.39 [vc-seq.setl])

Textually #includes:
vc-decode.setl     (Section A.10 [vc-decode.setl])
vc-exit.setl     (Section A.15 [vc-exit.setl])
vc-msg.setl     (Section A.30 [vc-msg.setl])

Source code: *

const yhwh = `vc-model.setl';
 
-- This is the main pump used by the do server, and maintains a
-- high-level model of the pan/tilt/zoom system.
--
-- It takes requests already checked and encapsulated as SETL maps,
-- performs them, and replies with similarly encapsulated responses.
--
-- It processes requests sequentially, but internally uses a ``sequencer''
-- program, vc-seq.setl, to take advantage of the possibility
-- of overlapped command-and-response (full duplex) communications with
-- the Canon VC-C3 to implement speed-ramped motion trajectories in
-- which pan/tilting and zooming are performed simultaneously.
 
-- Command sequencer:
const seq_fd = fileno open (`exec setl vc-seq.setl', `pump');
 
const sigterm_fd = open (`SIGTERM', `signal');  -- catch TERM signals
 
var cur_mode := `RC';  -- by assumption, but init_model changes it
 
var cur_zoom_factor := 1;
var cur_zoom_speed := 0;
var currently_zooming := false;
 
var cur_pan := 0, cur_tilt := 0;
var cur_pan_speed := 0, cur_tilt_speed := 0;
var currently_moving := false;
 
var ms_per_tick := 100;
const min_move_speed = 1;
const max_move_speed = 70;
const max_speed = max_move_speed * ms_per_tick / 1000;  -- deg/tick
const max_zoom_speed = 8;  -- the ``min'' is the -ve, and zooms out
 
const max_pan_speed = 76;  -- the ``min'' is the -ve, and pans left
const max_tilt_speed = 70;  -- the ``min'' is the -ve, and tilts down
 
const min_zoom = 1;
const max_zoom = 10;
 
var cur_ramp := 500;  -- ms
var ramp_ticks := ms_to_ticks cur_ramp;
var accel := max_speed / (1 max ramp_ticks);  -- speed change per tick
 
tie (stdinstdout);
 
init_model;
 
loop doing
  [ready] := select ([{sigterm_fdstdin}]);
  if sigterm_fd in ready then
    msg (yhwh + ` (' + str pid + `) caught SIGTERM');
    quit_gracefully;
  end if;
  read (cmd);
while not eof do
  write (perform (cmd));  -- perform cmd and give client the response
end loop;
 
proc init_model;
  do_init;
  do_mode_host;
  do_zoom_stop;
  do_move_stop;
  do_zoom_speed (5);  -- a medium-high default speed for zooms
end proc;
 
proc perform (cmd);
  r := do_response ([ ]);  -- default
  case cmd.name of
  (`Zoom'):
    case cmd.subcmd of
    (`Start'):  r := do_zoom_start();
    (`Stop'):   r := do_zoom_stop();
    (`Speed'):  r := do_zoom_speed (cmd.zoom_speed);
    (`To'):     r := do_zoom_to (cmd.zoom_factorcmd.speed);
    (`By'):     r := do_zoom_by (cmd.zoom_scale,  cmd.speed);
    else msg (`Unrecognized Zoom subcmd '+str cmd.subcmd);
    end case;
  (`Move'):
    case cmd.subcmd of
    (`Start'):  r := do_move_start();
    (`Stop'):   r := do_move_stop();
    (`Speed'):  r := do_move_speed (cmd.pan_speedcmd.tilt_speed);
    (`To'):     r := do_move_to (cmd.pancmd.tiltcmd.mscmd.speed);
    (`By'):     r := do_move_by (cmd.pancmd.tiltcmd.mscmd.speed);
    else msg (`Unrecognized Move subcmd '+str cmd.subcmd);
    end case;
  (`Jump'):
    case cmd.subcmd of
    (`To'):  r := do_jump_to (cmd.pancmd.tilt);
    --- still to implement:  'By'
    else msg (`Unrecognized Jump subcmd '+str cmd.subcmd);
    end case;
  (`Ramp'):
    r := do_ramp (cmd.ms);
  (`Mode'):
    case cmd(`mode') of  -- i.e., `cmd.mode', but mode is a keyword
    (`Host'):  r := do_mode_host();
    (`RC'):    r := do_mode_rc();
    else msg (`Unrecognized mode '+str cmd(`mode'));
    end case;
  (`Clear'):
    r := do_clear();
  (`Reload'):
    r := do_reload();
  (`Setup'):
    r := do_setup();
  (`Reset'):
    r := do_reset();
  (`Check'):
    r := do_check();
  (`Hex'):
    r := do_hex (cmd.cmd);
  (`Get'):
    r := do_get (cmd.what);
  else
    msg (`Unrecognized cmd name '+str cmd.name);
  end case;
  return r;
end proc perform;
 
proc do_zoom_start();
  [dev_dirdev_speed] := downcvt_zoom_speed (cur_zoom_speed);
  if dev_dir = `\xffthen  -- zero zoom speed => stop zooming
    return do_zoom_stop();
  end if;
  if not currently_zooming then
    do_cmd (unhex `011201'            -- device Start TELE or WIDE
                   + dev_dir);
    currently_zooming := true;
  end if;
  return do_response ([`Zoom Start']);
end proc do_zoom_start;
 
proc do_zoom_stop();
  if currently_zooming then
    do_cmd (unhex `011203');            -- device Zoom Stop
    currently_zooming := false;
  end if;
  return do_response ([`Zoom Stop']);
end proc do_zoom_stop;
 
proc do_zoom_speed (zoom_speed);
  zoom_speed max:= -max_zoom_speed;
  zoom_speed min:= max_zoom_speed;
  [dev_dirdev_speed] := downcvt_zoom_speed (zoom_speed);
  if dev_dir = `\xffthen  -- zero zoom speed => stop zooming
    if currently_zooming then
      do_zoom_stop();
    end if;
  else
    if currently_zooming and
       dev_dir /= (downcvt_zoom_speed (cur_zoom_speed))(1) then
      -- a ``sign change'' in the zoom speed
      do_cmd (unhex `011203');          -- device Zoom Stop
      do_cmd (unhex `01120402'          -- device Zoom Speed
                     + char dev_speed);
      do_cmd (unhex `011201'            -- device Zoom Start
                     + dev_dir);
    else
      do_cmd (unhex `01120402'          -- device Zoom Speed
                     + char dev_speed);
    end if;
  end if;
  cur_zoom_speed := zoom_speed;
  return do_response ([`Zoom Speed ' + fixed (zoom_speed, 0, 1)]);
end proc do_zoom_speed;
 
proc do_zoom_to (zoom_factorat_speed);
  if currently_zooming then
    do_zoom_stop();
  end if;
  if at_speed /= om then
    do_zoom_speed (at_speed);
  end if;
  zoom_factor max:= min_zoom;
  zoom_factor min:= max_zoom;
  dev_factor := downcvt_zoom_factor (zoom_factor);
  -- 3 to 12 seconds, depending:
  time_limit := 1000 * abs (zoom_factor - cur_zoom_factor) /
                                  (1 max abs cur_zoom_speed) + 3000;
  do_cmd (unhex `01120202'              -- device Zoom To
                 + to_two_bytes dev_factortime_limit);
  cur_zoom_factor := zoom_factor;
  return do_response ([`Zoom ' + fixed (zoom_factor, 0, 3) +
          if at_speed /= om then ` At ' + whole (cur_zoom_speed, 0)
          else `'
          end if]);
end proc do_zoom_to;
 
proc do_zoom_by (zoom_scaleat_speed);
  return do_zoom_to (zoom_scale * cur_zoom_factorat_speed);
end proc do_zoom_by;
 
proc do_move_start();
  [dev_pan_dirdev_pan_speed] := downcvt_pan_speed (cur_pan_speed);
  [dev_tilt_dirdev_tilt_speed] := downcvt_tilt_speed (cur_tilt_speed);
  if dev_pan_dir = `\x00and  -- zero speeds => stop panning
     dev_tilt_dir = `\x00then
    return do_move_stop();
  end if;
  if not currently_moving then
    do_cmd (unhex `051201'            -- device Pan/Tilt Start
                   + dev_pan_dir
                   + dev_tilt_dir);
    currently_moving := true;
  end if;
  return do_response ([`Move Start']);
end proc do_move_start;
 
proc do_move_stop();
  if currently_moving then
    do_cmd (unhex `051202');            -- device Pan/Tilt Stop
    currently_moving := false;
  end if;
  return do_response ([`Move Stop']);
end proc do_move_stop;
 
proc do_move_speed (pan_speedtilt_speed);
  pan_speed max:= -max_pan_speed;
  pan_speed min:= max_pan_speed;
  tilt_speed max:= -max_tilt_speed;
  tilt_speed min:= max_tilt_speed;
  [dev_pan_dirdev_pan_speed] := downcvt_pan_speed (pan_speed);
  [dev_tilt_dirdev_tilt_speed] := downcvt_tilt_speed (tilt_speed);
  if dev_pan_dir = `\x00and  -- zero speeds => stop panning
     dev_tilt_dir = `\x00then
    if currently_moving then
      do_move_stop();
    end if;
  else
    [cur_dev_pan_dir, -] := downcvt_pan_speed (cur_pan_speed);
    [cur_dev_tilt_dir, -] := downcvt_tilt_speed (cur_tilt_speed);
    if currently_moving and
       (dev_pan_dir /= cur_dev_pan_dir or
        dev_tilt_dir /= cur_dev_tilt_dirthen
      -- a ``sign change'' in the pan and/or tilt speed
      do_cmd (unhex `051202');                -- device Pan/Tilt Stop
      do_cmd (unhex `05120302'                -- device Pan/Tilt Speed
                     + char dev_pan_speed
                     + char dev_tilt_speed);
      do_cmd (unhex `051201'                  -- device Pan/Tilt Start
                     + dev_pan_dir
                     + dev_tilt_dir);
    else
      -- not moving, or no sign change; a simple speed change will do
      do_cmd (unhex `05120302'                -- device Pan/Tilt Speed
                     + char dev_pan_speed
                     + char dev_tilt_speed);
    end if;
  end if;
  cur_pan_speed := pan_speed;
  cur_tilt_speed := tilt_speed;
  return do_response ([`Move Speed ' + fixed (pan_speed, 0, 1) +
                               ` ' + fixed (tilt_speed, 0, 1)]);
end proc do_move_speed;
 
proc do_move_to (pantiltmsat_speed);
  assert ms = om or at_speed = om;  -- or both
  if currently_moving then
    do_move_stop();
  end if;
  -- It's no good planning trajectories to places we can't go, so
  -- clamp the request in case the caller hasn't bothered to deal with
  -- that yet, using the published limits:
  pan max:= -90;
  pan min:= +90;
  tilt max:= -30;
  tilt min:= +25;
  dpan := pan - cur_pan;
  dtilt := tilt - cur_tilt;
  deg := sqrt (dpan**2 + dtilt**2);
  ymid := deg / 2;
  -- The time axis is called x here, often indexed by integer k
  if ymid > 0 then
    dircos := dpan / deg;
    dirsin := dtilt / deg;
    ramp_ticks := ms_to_ticks cur_ramp;
    accel := max_speed / (1 max ramp_ticks);  -- speed change per tick
    n := 1 max ceil ramp_ticks;  -- a point at the ramp end or beyond
    -- Obtain xmid
    if at_speed /= om then
      at_speed max:= min_move_speed;
      at_speed min:= max_move_speed;
      speed := at_speed * ms_per_tick/1000;  -- deg/sec -> deg/tick
      assert speed > 0;
      assert exists k in [n-1,n-2..0] | k*accel < speed;
      speed min:= (k+1)*accel;
      xmid := k + (ymid-max_traj(k)) / speed;
      xmid := (ceil (2 * xmid)) / 2;
    else
      trajtime := ms ? 0;
      trajtime max:= 0;
      trajtime min:= 200000;  -- 200 deg at 1 deg/sec = 200 sec
      xmid := (ceil ms_to_ticks trajtime) / 2;
      if ymid > max_traj(xmidthen
        -- We cannot get there fast enough; revise xmid eastward
        -- accordingly by setting xmid to where the max-speed
        -- trajectory (see proc max_traj) reaches ymid and then
        -- rounding xmid up to the nearest half-tick
        assert exists k in [n-1,n-2..0] | max_traj(k) < ymid;
        speed := ((k+1)*accelmin max_speed;
        xmid := k + (ymid-max_traj(k)) / speed;
        xmid := (ceil (2 * xmid)) / 2;
        -- Now we can reach ymid in this upwardly revised time xmid
      end if;
    end if;
    -- xmid was or is okay now
    m := (n min ceil xmid) - 1;
    assert m >= 0;  -- else degenerate case slipped through
    -- Find the point (x = k) of departure from the max traj
    assert exists k in [m,m-1..0] | traj(xmid,k,k*accel) < ymid;
    -- Mid-trajectory speed is slope going from there up to xmidymid
    speed := (ymid-max_traj(k)) / (xmid-k);
    -- Trajectory function is now max_traj (x) for x <= k, and
    -- traj (xkspeed) for x >= k (when x = k, take your pick)
    ramp_cmds := [move_speed_cmd (accel*xdircosdirsin) :
                                        x in [1..k]];
    x_end := round (2 * xmid);  -- 2 * xmid already integer in theory
    cmds := [[x-1,     ramp_cmds (x)] : x in [1..k]] +
            [[k,       move_speed_cmd (speeddircosdirsin)]] +
            [[x_end-xramp_cmds (x)] : x in [k,k-1..1]] +
            [[x_end,   move_stop_cmd()]];
    cmds(2..1) := [[0, move_start_cmd (speeddircosdirsin)]];
    -- cmds = [[ticknum, cmd], ...]
    time_limit := 2000;  -- allow 2 sec after final Stop
    responses := do_cmds (cmdsms_per_ticktime_limit);
    if #responses /= #cmds then
      msg (str #cmds+` commands sent but '+str #responses+
                                              ` responses received');
    else
      for response = responses(iloop
        [-,cmd] := cmds(i);
        if not (response satisfies cmdthen
          report (responsecmd);
        end if;
      end loop;
    end if;
  end if ymid;
  -- Have the camera ``settle'' to its final position at a speed
  -- suitable for the current zoom factor:
  zorp (pantiltmax_zoom / cur_zoom_factor);
  return do_response ([`Move To ' + fixed (pan, 0, 3) +
                             ` ' + fixed (tilt, 0, 3) +
          if at_speed /= om then ` At ' + whole (at_speed, 0)
          elseif ms /= om then ` In ' + whole (ms, 0)
          else `'
          end if]);
end proc do_move_to;
 
proc do_move_by (dpandtiltmsat_speed);
  return do_move_to (cur_pan + dpancur_tilt + dtiltmsat_speed);
end proc do_move_by;
 
proc move_speed_cmd (speeddircosdirsin);
  deg_per_sec := speed * 1000/ms_per_tick;
  [-, dev_pan_speed] := downcvt_pan_speed (deg_per_sec * dircos);
  [-, dev_tilt_speed] := downcvt_tilt_speed (deg_per_sec * dirsin);
  return unhex `05120302'                 -- device Pan/Tilt Speed
                + char dev_pan_speed
                + char dev_tilt_speed;
end proc;
 
proc move_start_cmd (speeddircosdirsin);
  deg_per_sec := speed * 1000/ms_per_tick;
  [dev_pan_dir, -] := downcvt_pan_speed (deg_per_sec * dircos);
  [dev_tilt_dir, -] := downcvt_tilt_speed (deg_per_sec * dirsin);
  return unhex `051201'                   -- device Pan/Tilt Start
                + dev_pan_dir
                + dev_tilt_dir;
end proc;
 
proc move_stop_cmd();
  return unhex `051202';                  -- device Pan/Tilt Stop
end proc;
 
proc do_jump_to (pantilt);
  zorp (pantiltmax_move_speed);
  return do_response ([`Move To ' + fixed (pan, 0, 3) +
                             ` ' + fixed (tilt, 0, 3)]);
end proc do_jump_to;
 
-- Utility for do_move_todo_jump_to
  do_move_status();  -- reload cur_pancur_tilt from hardware
  dpan := pan - cur_pan;
  dtilt := tilt - cur_tilt;
  deg := sqrt (dpan**2 + dtilt**2);
  if deg > 0 then
    dircos := dpan / deg;
    dirsin := dtilt / deg;
    -- Base the time limit on the reasonable assumption of greater error
    -- over longer trajectories:
    do_move_speed (settle_speed * dircossettle_speed * dirsin);
    settle_time := 1000 + 200 * deg / settle_speed;
    dev_pan := downcvt_pan (pan);
    dev_tilt := downcvt_tilt (tilt);
    do_cmd (unhex `05120502'              -- device Pan/Tilt To
                   + to_two_bytes dev_pan
                   + to_two_bytes dev_tiltsettle_time);
    cur_pan := pan;
    cur_tilt := tilt;
  end if;
end proc zorp;
 
proc do_ramp (ms);
  ms max:= 0;
  ms min:= 15000;  -- even a 15-second ramp is pretty incredibly slow
  cur_ramp := ms;
  return do_response ([`Ramp ' + str ms]);
end proc do_ramp;
 
proc do_mode_host();
  do_clear;  -- toggle RTS
  do_cmd (unhex `08170100');    -- device Mode Select PC
  cur_mode := `Host';
  -- N.B. do_setup auto-detects home and re-reads pan/tilt/zoom info:
  return do_response ([`Mode Host'] + do_setup().notices);
end proc do_mode_host;
 
proc do_mode_rc();
  do_clear;  -- toggle RTS
  do_cmd (unhex `08170101');    -- device Mode Select Remote Controller
  cur_mode := `RC';
  return do_response ([`Mode RC']);
end proc do_mode_rc;
 
proc do_init();
  -- Condition the serial line
  do_cmd (`i');  -- ``initialize''
  return do_response ([ ]);
end proc do_init;
 
proc do_clear();
  -- Lower the RTS line for 100 ms, then raise it
  do_cmd (`c');  -- ``clear''
  return do_response ([ ]);
end proc do_clear;
 
proc do_reload();
  --
  -- Note that the model's impression of whether the platform is
  -- currently pantilting or zooming is not updated by this reload,
  -- since I don't know how to read those bits from the hardware.
  --
  -- Moreover, since directions are bound up with the zoom and
  -- pan/tilt Start subcommands, I can't read those either, so my
  -- update of the zoom and pan and tilt speeds just assumes the
  -- sign is whatever I already think it is.
  --
  -- Similarly I must shrug and not update the current control mode
  -- (Host or RC).
  --
  return do_response (do_zoom_status().notices +
                      do_move_status().notices);
end proc do_reload;
 
proc do_zoom_status();
  cmd := unhex `010402';               -- device Zoom Status
  response := do_cmd (cmd);
  if is_string response and #response = 6 and response(2) = `\x84then
    dev_zoom_speed  := abs response(4);
    dev_zoom_factor := from_two_bytes response(5..6);
    [dev_zoom_dir, -] := downcvt_zoom_speed (cur_zoom_speed);
    cur_zoom_speed  := upcvt_zoom_speed (dev_zoom_dirdev_zoom_speed);
    cur_zoom_factor := upcvt_zoom_factor (dev_zoom_factor);
    return do_response ([`Zoom Speed ' + fixed (cur_zoom_speed, 0, 1),
                         `Zoom '      + fixed (cur_zoom_factor, 0, 3)]);
  end if;
  msg (`Unexpected response ' + str decode response +
       ` to Zoom Status command ' + str decode cmd);
  return do_reset_if_negative_response (response);
end proc do_zoom_status;
 
proc do_move_status();
  cmd := unhex `050402';               -- device Pan/Tilt Status
  response := do_cmd (cmd);
  if is_string response and #response = 9 and response(2) = `\x84then
    dev_pan_speed  := abs response(4);
    dev_tilt_speed := abs response(7);
    dev_pan        := from_two_bytes response(5..6);
    dev_tilt       := from_two_bytes response(8..9);
    [dev_pan_dir, -] := downcvt_pan_speed (cur_pan_speed);
    cur_pan_speed  := upcvt_pan_speed (dev_pan_dirdev_pan_speed);
    [dev_tilt_dir, -] := downcvt_tilt_speed (cur_tilt_speed);
    cur_tilt_speed := upcvt_tilt_speed (dev_tilt_dirdev_tilt_speed);
    cur_pan        := upcvt_pan (dev_pan);
    cur_tilt       := upcvt_tilt (dev_tilt);
    return do_response ([`Move Speed ' + fixed (cur_pan_speed, 0, 1) +
                                  ` ' + fixed (cur_tilt_speed, 0, 1),
                         `Move To ' + fixed (cur_pan, 0, 3) +
                               ` ' + fixed (cur_tilt, 0, 3)]);
  end if;
  msg (`Unexpected response ' + str decode response +
       ` to Pan/Tilt Status command ' + str decode cmd);
  return do_reset_if_negative_response (response);
end proc do_move_status;
 
proc do_reset_if_negative_response (frame);
  -- This is a hack, obviously.  It turns out that when the Canon
  -- starts responding to Status requests with ``negative response'',
  -- it will continue to do so.  Whether this is dB's fault or a glitch
  -- in the Canon firmware remains unknown.  Fortunately, it happens
  -- rarely, much less than once per day in the early months of testing.
  -- Whatever the etiology, a remedy that works is to pull out the
  -- hammer and do a Reset whenever this condition is detected.
  if #frame >= 2 and
     (abs frame(1) bit_and 16#80) = 0 and
     (abs frame(2) bit_and 16#60) = 16#40 then
    return do_reset();
  else
    return do_response ([ ]);
  end if;
end proc;
 
proc do_setup();
  cmd := unhex `0510';                 -- device Pan/Tilt Setup
  response := do_cmd (cmd, 5000);  -- 5 sec time limit
  if is_string response and #response = 6 and response(2) = `\x90then
    pass;  -- response(3..6) has the absolute position, but
           -- do_reload will pick that up in a moment anyway
  else
    msg (`Unexpected response ' + str decode response +
         ` to Pan/Tilt Setup command ' + str decode cmd);
  end if;
  return do_reload();  -- refresh model state from hardware state
end proc do_setup;
 
proc do_reset();
  do_init;
  do_cmd (`a', 6000);  -- provoke VC-C3 auto-init, 6 sec time limit
  return do_mode_host();
end proc do_reset;
 
proc do_check();
  zoom_factor := cur_zoom_factor;
  [pantilt] := [cur_pancur_tilt];
  do_reload();
  --- move these fuzz constants out to a more respectable place:
  if abs (cur_pan - pan) > 0.25 or
     abs (cur_tilt - tilt) > 0.25 or
     --- It's a fiction that this is a zoom ``factor''.
     --- It almost certainly is just the position of the servo
     --- controlling the zoom ring:
     abs (cur_zoom_factor - zoom_factor) > 0.05 then
    return do_reset();
  end if;
  return do_response ([ ]);
end proc do_check;
 
proc do_hex (cmd);  -- unadvertised, for low-level experimentation
  response := do_cmd (unhex cmd, 20000);  -- 20-second time limit (wow)
  return do_response ([`Frame ' + str decode response]);
end proc do_hex;
 
proc do_get (what);
  r := do_response ([ ]);
  r.value := case what of
  (`mode'):         cur_mode,
  (`zoom_factor'):  cur_zoom_factor,
  (`zooming'):      currently_zooming,
  (`zoom_speed'):   cur_zoom_speed,
  (`position'):     [cur_pancur_tilt],
  (`moving'):       currently_moving,
  (`move_speed'):   [cur_pan_speedcur_tilt_speed],
  (`ramp'):         cur_ramp
  else msg (`Unrecognized Get argument '+str what)
  end case;
  return r;
end proc do_get;
 
-- The model replies to the client with a record including `notices':
proc do_response (notices);
  r := {};
  r.notices := notices;
  return r;
end proc;
 
 
proc do_cmd (cmdtime_limit(*));  -- do command and get lo-lev response
  seq := {};
  seq.cmd := cmd;
  -- 3-second response timeout default:
  seq.time_limit := time_limit(1) ? 3000;
  return do_step (seq);
end proc;
 
proc do_cmds (cmdstick_mstime_limit(*));  -- many cmds, responses
  seq := {};
  seq.cmds := cmds;
  seq.tick_ms := tick_ms;
  -- 4.5-second response timeout default:
  seq.time_limit := time_limit(1) ? 4500;
  return do_step (seq);
end proc;
 
proc do_step (seq);  -- send a command-sequence packet and get response
  writea (seq_fdseq);
  reada (seq_fdresponse);
  if eof then
    msg (yhwh + ` got EOF from sequencer - quitting');
    quit_gracefully;
  end if;
  return response;
end proc;
 
op satisfies (responsecmd);
  assert is_string cmd;
  if response = cmd then return trueend if;
  if not is_string response then return falseend if;
  if #response >= 2 and #cmd >= 2 and
     response(1) + char (abs response(2) bit_and 16#3f#) +
     response(3..#response min #cmd) = cmd then
    return true;
  end if;
  return false;
end op;
 
proc report (responsecmd);
  if not is_string response then
    msg (`Non-string response '+str response+
                 ` to command '+str decode cmd);
  else
    msg (`Unexpected response '+str decode response+
                 ` to command '+str decode cmd);
  end if;
end proc;
 
 
-- General note about speed conversions:  ``down'' conversions always
-- yield a sign as a single character suitable for plugging into a
-- device-level command, and a magnitude that is a positive integer
-- in the range 0..65535 suitable for passing to two_bytes.
 
 
-- Zoom rate conversions:
--
-- The zoom hardware requires a direction and a 3-bit magnitude.
--
-- But since that does not provide for a zero, I will use as
-- the ``direction'' a value of `\xff' rather than one of the valid
-- values `\x00' (TELE) or `\x01' (WIDE) if I think the zoom speed
-- you supply is about 0.
--
-- I don't yet know the relationship between the 8 available speeds
-- in each direction and the actual change in zoom factor per second,
-- so for now the mapping is just this:
--
-- I take your zoom_speed spec and map it to the nearest integer n
-- in -8..8.  If n is 0, you get dev_zoom_dir = `\xff' (and
-- dev_zoom_speed is om), but otherwise you get
-- dev_zoom_dir = `\x00' for positive and `\x01' for negative,
-- and dev_zoom_speed will be one less than the magnitude of n.
--
-- Going the other way, I take one of those three direction indicators
-- together with a speed in the range 0..7, and map the pair to
-- -8..-8 (8 is symbolized as max_zoom_speed in the code).
 
proc downcvt_zoom_speed (zoom_speed);
  n := round zoom_speed max -max_zoom_speed min max_zoom_speed;
  case sign n of
  (-1):  return [`\x01', -n-1];  -- i.e., abs n - 1
  (0):   return [`\xff', om];
  (1):   return [`\x00', n-1];
  end case;
end proc;
 
proc upcvt_zoom_speed (dev_zoom_dirdev_zoom_speed);
  case dev_zoom_dir of
  (`\x01'):  return -dev_zoom_speed - 1;
  (`\xff'):  return 0;
  (`\x00'):  return dev_zoom_speed + 1;
  end case;
end proc;
 
-- Zoom factor conversions:
--
-- Until I know the relationship between zoom ring position
-- and zoom factor, I will just scale linearly; I might try
-- logarithmically later if it looks like it might work out
-- better.
 
#define zoom_lo min_zoom
#define zoom_hi max_zoom
#define dev_zoom_lo 0
#define dev_zoom_hi 16#469#
 
proc downcvt_zoom_factor (zoom_factor);
  u := ((zoom_factor - zoom_lo) /
        (zoom_hi     - zoom_lo)) max 0 min 1;
  -- Rounding is not quite ``fair'' here but I like it anyway
  return round (u * (dev_zoom_hi - dev_zoom_lo) + dev_zoom_lo);
end proc;
 
proc upcvt_zoom_factor (dev_zoom_factor);
  u := ((dev_zoom_factor - dev_zoom_lo) /
        (dev_zoom_hi     - dev_zoom_lo)) max 0 min 1;
  return u * (zoom_hi - zoom_lo) + zoom_lo;
end proc;
 
-- Pan/Tilt rate conversions:
--
-- The device units, degrees per second, seem ideal.  ``Down''
-- conversions map your signed pan_speed or tilt_speed to a
-- [sign, magnitude] pair.  The magnitude is suitable for a device
-- Pan/Tilt Speed command, and the sign for Pan/Tilt Start.
--
-- The speed range is -76 to 76 degrees per second for pan, and
-- -70 to 70 for tilt, with positive meaning rightgoing or upgoing
-- respectively.
--
-- ``Up'' conversions are the obvious inverse of down, without the
-- range check or rounding.
 
proc downcvt_pan_speed (pan_speed);
  n := round pan_speed max -max_pan_speed min max_pan_speed;
  case sign n of
  (-1):  return [`\x02', -n];  -- i.e., abs n
  (0):   return [`\x00', 1];
  (1):   return [`\x01', n];
  end case;
end proc;
 
proc upcvt_pan_speed (dev_pan_dirdev_pan_speed);
  case dev_pan_dir of
  (`\x02'):  return -dev_pan_speed;
  (`\x00'):  return 0;
  (`\x01'):  return dev_pan_speed;
  end case;
end proc;
 
proc downcvt_tilt_speed (tilt_speed);
  n := round tilt_speed max -max_tilt_speed min max_tilt_speed;
  case sign n of
  (-1):  return [`\x02', -n];  -- i.e., abs n
  (0):   return [`\x00', 1];
  (1):   return [`\x01', n];
  end case;
end proc;
 
proc upcvt_tilt_speed (dev_tilt_dirdev_tilt_speed);
  case dev_tilt_dir of
  (`\x02'):  return -dev_tilt_speed;
  (`\x00'):  return 0;
  (`\x01'):  return dev_tilt_speed;
  end case;
end proc;
 
-- Pan/Tilt conversions:
--
-- The natural units are degrees.  The Canon takes units of 0.115 deg
-- in both pan and tilt.  Hex 8000 is the midpoint (home), and you can
-- go from hex -30E to 30E in pan, -10A to 0D9 in tilt, representing
-- about -90 to 90 deg in pan, -30 to 25 deg in tilt.
--
-- The device convention for pan is ``backwards''.
 
proc downcvt_pan (pan);
  return 16#8000 + round ((-pan / 0.115) max -16#30E min 16#30E);
end proc;
 
proc upcvt_pan (dev_pan);
  return (16#8000 - dev_pan) * 0.115;
end proc;
 
proc downcvt_tilt (tilt);
  return 16#8000 + round ((tilt / 0.115) max -16#10A min 16#0D9);
end proc;
 
proc upcvt_tilt (dev_tilt);
  return (dev_tilt - 16#8000) * 0.115;
end proc;
 
 
-- Trajectory functions
 
proc max_traj (x);  -- value of ramp or its max_speed extension at x
  assert x >= 0;
  n := 1 max ceil ramp_ticks;  -- a point at the ramp end or beyond
  k := ceil x;
  if k < n then
    return accel*k*(x-(k-1)/2);  -- if x=k, this is accel*k*(k+1)/2
  else
    -- k>=n means x is beyond what the ramp covers.  Follow the ramp
    -- to n-1 and then travel at max_speed the rest of the way to x:
    return traj (xn-1, max_speed);
  end if;
end proc max_traj;
 
proc traj (xkspeed);  -- follow ramp to k, then go maxly to x
  assert is_integer k;
  assert k >= 0;
  assert x >= k;  -- I don't need this, but else what a strange caller!
  return max_traj (k) + speed*(x-k);
end proc;
 
op ms_to_ticks (ms);  -- for example, k and x above are in ticks
  return ms / ms_per_tick;
end op;
 
 
-- Endianness-independent packer and unpacker of two-byte spam
 
op to_two_bytes (i);
  return char (i div 256) + char (i mod 256);
end op;
 
op from_two_bytes (s);
  return abs s(1) * 256 + abs s(2);
end op;
 
 
proc quit_gracefully;
  exit_gracefully ([[str filename seq_fdseq_fd]]);
end proc;
 
 
#include ``vc-decode.setl''
#include ``vc-exit.setl''
#include ``vc-msg.setl''


next up previous
Next: A.28 vc-mouse.setl Up: A. WEBeye Source Code Previous: A.26 vc-master.cgi
David Bacon
1999-12-10