////////////////////////////////////////////////////////////////////////
//
//  Movado: Pico-2 motion controller (PID control at 1000Hz)
//      version 1.3 (July 7, 2009)
//
//  (C) Hideki Kozima (xkozima@myu.ac.jp), subject to GPLv2
//
//  For technical information
//      http://www.carebots.org/robot-clayspec.html
//
//  Packet structure
//      6-byte sequence:
//          1,addr:5,n:2
//          0,inst:3,msbs:4
//          0,hept1:7
//          0,hept2:7
//          0,hept3:7
//          0,hept4:7
//      Elements of a packet:
//          addr: node address (0x00..0x1f)
//              note: for addr in 0x00..0x07 -> Nenc=16
//                    for addr in 0x08..0x0f -> Nenc=200
//                    for addr in 0x10..0x1e -> Nenc=256
//              note: addr=0x1f (31) for broadcast to all nodes
//          n   : motor number (0,1,2,3)
//          inst: instruction (0-7)
//          msbs: bit7s for hept1-4
//          hept: constitute "quad" (data1-4) with msbs
//              data1 = ((msbs & 0x08)? 0x80: 0) | hept1;
//              data2 = ((msbs & 0x04)? 0x80: 0) | hept2;
//              data3 = ((msbs & 0x02)? 0x80: 0) | hept3;
//              data4 = ((msbs & 0x01)? 0x80: 0) | hept4;
//          quad (4bytes = 32bits):
//              quad:32 = (data1<<24) | (data2<<16) | (data3<<8) | data4;
//
//  Fixed-point number
//      s1616 (fix): integer:16,fraction:16 <-> quad:32
//      s0824      : integer:08,fraction:24 (for ERPC8, DTf8)
//      s2408      : integer:24,fraction:08 (for internal use)
//
//  Command syntax/semantics
//      inst=0 (move)                   //  move to pos [rad] in CAM
//          fix  pos:32 = quad;         //  (Constant-Acceleration Mode)
//          move(addr, n, pos);
//      inst=1 and quad<0 (movac)       //  move to pos with acc in CAM
//          fix  acc:32 = -(quad & 0xffff0000);
//          fix  pos:32 = (quad << 16);
//          movac(addr, n, acc, pos);
//      inst=2 and quad<0 (movel)       //  move to pos with vel in CAM
//          fix  vel:32 = -(quad & 0xffff0000);
//          fix  pos:32 = (quad << 16);
//          movel(addr, n, vel, pos);
//      inst=1 and quad>=0 (setacc)     //  set acc [rad/s/s] for move
//          fix  acc:32 = quad;
//          setacc(addr, n, acc);
//      inst=2 (setvel)                 //  set max vel [rad/s] for move/mova
//          fix  vel:32 = quad;
//          setvel(addr, n, vel);
//      inst=3 (setpos)                 //  do ballistic motion to pos [rad]
//          fix  pos:32 = quad;
//          setpos(addr, n, pos);
//      inst=4 (sub)                    //  sub-commands (see below)
//          uchar  jnst:8  = data1;
//          uchar  index:8 = data2;
//          ushort word:16 = (data3 << 8) | data4;
//          sub(addr, n, jnst, index, word);
//      inst=5 (getacc)                 //  return acc [rad/s/s] for move/mova
//          fix  acc:32 = getacc(addr, n);
//          return(addr, n, acc);
//      inst=6 (getvel)                 //  return current vel [rad/s]
//          fix  vel:32 = getvel(addr, n);
//          return(addr, n, vel);
//      inst=7 (getpos)                 //  return current pos [rad]
//          fix  pos:32 = getpos(addr, n);
//          return(addr, n, pos);
//
//  Sub-command syntax/semantics
//      inst  jnst    index     word
//      100 00000000  0<index>  <upper> set_upper 16bits to F[index][n]
//      100 00000000  11111111  <lower> set_lower 16bits (call this first)
//      100 00000001  <=index>  <word>  set_word to W[index][n]
//      100 01110000  00000000  <pos>   move_rcs: move to pos in CAM
//      100 01110000  00000001  <acc>   setacc_rcs: set acc for move_rcs
//      100 01110000  00000010  <vel>   setvel_rcs: set vel for move_rcs
//      100 01110000  00000011  <pos>   setpos_rcs: ballistic motion
//      100 01111111  00000000  ---     set_zero (set zero position)
//      100 01111111  00000001  <mode>  set_mode to MODE
//      100 01111111  00000010  <camod> set_camod to CAMOD
//      100 01111111  00000100  ---     do_cen (go to center = (MIN+MAX)/2)
//      100 01111111  00000101  <emg>   do_max (find max with VCAM and EMG)
//      100 01111111  00000110  <emg>   do_min (find min with VCAM and EMG)
//      100 01111111  00000111  <emg>   do_minmax (do_min then do_max)
//      100 01111111  00001000  <emg>   do_minmaxcen (do_minmax, cen, zero)
//      100 01111111  00001001  <emg>   do_minmaxmax (do_minmax, max, zero)
//      100 01111111  00001010  <emg>   do_minmaxmin (do_minmax, min, zero)
//      100 01111111  11111111  ---     reset Movado
//      100 10000000  0<index>  *<up>   get_upper 16bits from F[index][n]
//      100 10000000  11111111  *<lo>   get_lower 16bits (call this later)
//      100 10000001  <=index>  *<word> get_word from W[index][n]
//      100 10000010  ---       *<ver>  get_version (ex.0x0102 for "1.2")
//      100 11110000  00000101  *<acc>  getacc_rcs: return acc for rcs
//      100 11110000  00000110  *<vel>  getvel_rcs: return vel for rcs
//      100 11110000  00000111  *<pos>  getpos_rcs: return pos for rcs
//      100 11111111  <=inter>  <count> get_report (reports pos sequence)
//
//  Response packet
//      Movado returns a response packet for the following commands.
//      (Response packets do not change addr and n in the query packets.)
//          inst    quad
//          101     *<acc:32>                 getacc <- current acc
//          110     *<vel:32>                 getvel <- current vel
//          111     *<pos:32>                 getpos <- current pos
//      Movado returns a response packet for the following sub-commands.
//      (Response packets do not change addr, n, jnst, index.)
//          100 10000000  0<index>  *<up>     get_upper: up <- F[index][n]>>16
//          100 10000000  11111111  *<lo>     get_lower: lo <- lower 16bits
//          100 10000001  <=index>  *<word>   get_word: word <- W[index][n]
//          100 10000011  ---       *<ver>    get_version: ver <- VER
//      Movado returns a sequence of packets for "get_report" sub-command.
//      (Response packets do not change addr,n for this sub-command.)
//          100 11111111  <=inter>  <count>   get_report: quad <- pos
//              inter: the interval time [ms] for reporting
//              count: the number of packets to send back
//          After having received a "get_report" sub-command, 
//          Movado waits for "move", "mova", or "setpos" command, 
//          which triggers sending back of <count> packets
//          with the interval of <inter> [ms].
//
//  Indices for F (fix)
//      XPOS        0x00        goal position (rad)
//      XVEL        0x01        velocity in CAM (rad/s)
//      ACAM        0x02        acceleration in CAM (rad/s/s)
//      VCAM        0x03        max velocity in CAM (rad/s)
//      PCAM        0x04        goal position in CAM (rad)
//      RXPOS       0x05        RCS goal position (0x24000000 for 1.5ms)
//      RXVEL       0x06        RCS velocity in CAM
//      RACAM       0x07        RCS acceleration in CAM
//      RVCAM       0x08        RCS max velocity in CAM
//      RPCAM       0x09        RCS goal position in CAM
//      PMAR        0x0a        safe margin in rad
//      POS         0x0b        current position
//      VEL         0x0c        current velocity (after LPF)
//      XTOR        0x0d        goal torque (EXPERIMENTAL)
//      TOR         0x0e        current torque (EXPERIMENTAL)
//      ERPC8       0x0f        encoder resolution (s0824)
//      MIN         0x10        min position (set by MMF)
//      MAX         0x11        max position (set by MMF)
//      KP          0x12        gain for proportional control
//      KI          0x13        gain for integral control
//      KD          0x14        gain for differential control
//      VMP         0x15        motor power voltage (in V)
//      VMAG        0x16        voltage magnifier (auto set: 32768 / VMP)
//      VINT        0x17        accumlator for integral cont
//      POLD        0x18        old position for velocity calc
//      LPFA        0x19        alpha factor (for LPF(VEL))
//      LPFD        0x1a        decay factor (auto set: 1.0 - LPFA)
//      ACAM0       0x1b        acceleration for move commands
//      VCAM0       0x1c        max velocity for move commands
//
//  Indices for W (word)
//      CAMOD       0x00        submode in CAM
//      MODE        0x01        operation mode
//      RCAMOD      0x02        RCS submode in CAM
//      RMODE       0x03        RCS operation mode
//      MMFLG       0x04        minmax finder flag
//      MMEMG       0x05        minmax error magnitude
//      REP_INT     0x06        report interval (of Dt)
//      REP_CNT     0x07        report count (tmp)
//      REP_ACT     0x08        report count in action
//      REP_TIC     0x09        report interval tick
//

////////////////////////////////////////////////////////////////////////
//
//  fundamentals

#include "sh7046f.h"                    //  header for sh7046f
#include "bios.c"                       //  bios program for Pico-2

//  program version

#define  VER  0x0103                    //  ex. 0x010f for 1.15

//  Pico-2 controls 4 axes in parallel

#define  NN   4                         //  number of axes (motors)

//  packet length

#define  PACLEN  6                      //  packet length in bytes

//  address of this pico2 (5bits) <- to be set by SW5-1

uchar    ADDR;                          //  communication address

//  control cycle (DTf: period, FQf: frequency)

#define  DTf8 0x00004189                //  0.001s in s0824 format
#define  FQf  0x03e80000                //  1000Hz in s1616 format
#define  CMi  767                       //  CMT count for 4000Hz = 768*8P

//  fix type

typedef  int  fix;                      //  fixed point s1616

//
//  main of Movado

int  main ()
{
    void  VAR_init(void),               //  parameter initializer
          PAC_do(uchar*),               //  packet interpretor 
          ACT_do(void);                 //  actuation process

    //  check if rebooted from WOVF (watch-dog timer overflow)
    if (WDT_check()) {
        LED_set(0x1f);                  //  "help me" on LEDs
        for (;;);                       //  infinite loop
    }

    //  init LED, address
    LED_init();
    LED_set(0x00);
    SWT_init();
    ADDR = SWT_get();

    //  init variable block
    VAR_init();

    //  initialization for PWM/RCS/ENC
    PWM_init();
    RCS_init();
    ENC_init();

    //  SCI setup with address
    //      PACLEN=6 (1-addr5-n2, 0-inst3-msbs4, 
    //                0-hept7, 0-hept7, 0-hept7, 0-hept7 )
    SCI_init(ADDR, PACLEN, (void *) PAC_do);

    //  CMT setup (cselB2=0x00, CMi=767)
    //      1000Hz (control) -> 4000Hz (strobe) -> P/768  -> CMi=767
    CMT0_init(0x00, CMi, ACT_do);

    //  SEN setup (motor current sensors)
    //  SEN_init();

    //  WDT setup (csel=0x04: 2.6667ms=375Hz)
    WDT_init(0x04);

    //  start event-loop
    set_imask(0);
    for (;;) {
    }

    return 0;
}

////////////////////////////////////////////////////////////////////////
//
//  FIX: fixed point calculation (s1616)
//      xxxx:xxxx:xxxx:xxxx.xxxx:xxxx:xxxx:xxxx (32bits)
//      |                 | |                 |
//      2^15           2^0   2^-1         2^-16

fix  FIX_safe_s3216 (long long f64)
{
    if ((f64 & 0xffff80000000LL) == 0xffff80000000LL)
        return  f64 & 0xffffffffLL;     //  minus (normal)
    else if (f64 & 0x800000000000LL)
        return  0x80000000;             //  minus (out of range)
    else if (f64 & 0x7fff80000000LL)
        return  0x7fffffff;             //  plus (out of range)
    else
        return  f64;                    //  plus (normal)
}

fix  FIX_safe_s4816 (long long f64)
{
    if ((f64 & 0xffffffff80000000LL) == 0xffffffff80000000LL)
        return  f64 & 0xffffffffLL;     //  minus (normal)
    else if (f64 & 0x8000000000000000LL)
        return  0x80000000;             //  minus (out of range)
    else if (f64 & 0x7fffffff80000000LL)
        return  0x7fffffff;             //  plus (out of range)
    else
        return  f64;                    //  plus (normal)
}

fix  FIX_mul (fix f1, fix f2)
{
    long long  f64;

    f64 = (((long long) f1) * ((long long) f2)) >> 16;
    return  FIX_safe_s3216(f64);
}

fix  FIX_div (fix f1, fix f2)
{
    long long  f64;

    f64 = (((long long) f1) << 16) / ((long long) f2);
    return  FIX_safe_s4816(f64);
}

fix  FIX_add (fix f1, fix f2)
{
    long long  f64;

    f64 = ((long long) f1) + ((long long) f2);
    return  FIX_safe_s4816(f64);
}

fix  FIX_sub (fix f1, fix f2)
{
    long long  f64;

    f64 = ((long long) f1) - ((long long) f2);
    return  FIX_safe_s4816(f64);
}

////////////////////////////////////////////////////////////////////////
//
//  var: parameter blocks

//
//  fix quads (32bit fixed point s1616, packed for 4 axes)

#define  NF        29                   //  number of fix quads

fix  F[NF][NN];

#define  FO_MOT    0                    //  offset for DC-motor quads
#define  FO_RCS    5                    //  offset for RC-servo quads
                                        //  ( XPOS, XVEL, ACAM, VCAM, PCAM,
                                        //   RXPOS,RXVEL,RACAM,RVCAM,RPCAM )

#define  XPOS      0x00                 //  goal position
#define  XVEL      0x01                 //  velocity in CAM (in rad/s)
#define  ACAM      0x02                 //  acceleration in CAM
#define  VCAM      0x03                 //  max velocity in CAM
#define  PCAM      0x04                 //  goal position in CAM

#define  RXPOS     0x05                 //  RCS goal position
#define  RXVEL     0x06                 //  RCS velocity in CAM (in rad/s)
#define  RACAM     0x07                 //  RCS acceleration in CAM
#define  RVCAM     0x08                 //  RCS max velocity in CAM
#define  RPCAM     0x09                 //  RCS goal position in CAM

#define  RXPOS_DEF 0x24000000           //  default (center) for RXPOS
#define  RACAM_DEF 0x04000000           //  default RACAM (approx 30deg/s/s)
#define  RVCAM_DEF 0x04000000           //  RVCAM default (approx 30deg/s)

#define  PMAR      0x0a                 //  margin for MIN/MAX (in rad)
#define  PMAR_DEF  0x00010000           //  default PMAR (1.0rad)

#define  POS       0x0b                 //  current position
#define  VEL       0x0c                 //  current velocity (after LPF)
#define  XTOR      0x0d                 //  goal torque (EXPERIMENTAL)
#define  TOR       0x0e                 //  current torque (EXPERIMENTAL)

#define  ERPC8     0x0f                 //  encoder resolution (s0824)
#define  ERPC8_16  0x001921FB           //  rad/count (for Nenc=16)
#define  ERPC8_200 0x000202B8           //  rad/count (for Nenc=200)
#define  ERPC8_256 0x00019220           //  rad/count (for Nenc=256)

#define  MIN       0x10                 //  min position (set by MMF)
#define  MAX       0x11                 //  max position (set by MMF)
#define  MIN_DEF   0x80000000           //  default MIN position (MININT)
#define  MAX_DEF   0x7fffffff           //  default MAX position (MAXINT)

#define  KP        0x12                 //  gain for proportional control
#define  KI        0x13                 //  gain for integral control
#define  KD        0x14                 //  gain for differential control
#define  KP_DEF    0x00020000           //  default KP =  2.00000
#define  KI_DEF    0x00280000           //  default KI = 40.00000
#define  KD_DEF    0x00000ccd           //  default KD =  0.05000

#define  VMP       0x15                 //  motor power voltage (in V)
#define  VMP_DEF   0x000c0000           //  VMP default (for 12.0V)

#define  VMAG      0x16                 //  voltage magnifier (<= VAR_init)
                                        //    VMAG = 32768 / VMP

#define  VINT      0x17                 //  accumlator for integral cont

#define  POLD      0x18                 //  old position for velocity calc

#define  LPFA      0x19                 //  alpha factor (for LPF(VEL))
#define  LPFA_16   0x00001000           //    alpha = 1/16
#define  LPFA_200  0x00004000           //    alpha = 1/4
#define  LPFA_256  0x00004000           //    alpha = 1/4

#define  LPFD      0x1a                 //  decay factor (<= VAR_init)
                                        //    LPFD = 1.0 - LPFA

#define  ACAM0     0x1b                 //  acceleration for move commands
#define  ACAM0_DEF 0x00d406ae           //  default ACAM0 = 0.5*(2*Pi*67.49)

#define  VCAM0     0x1c                 //  max velocity for move commands
#define  VCAM0_DEF 0x00d406ae           //  default VCAM0 = 0.5*(2*Pi*67.49)

//
//  word block (16bit; unsigned short)

#define  NW        10                   //  number of word quads

ushort  W[NW][NN];

#define  WO_MOT    0                    //  offset for DC-motor quads
#define  WO_RCS    2                    //  offset for RC-servo quads
                                        //  ( CAMOD, MODE, 
                                        //   RCAMOD, RMODE )

#define  CAMOD     0x00                 //  submode in CAM
#define  MODE      0x01                 //  operation mode

#define  RCAMOD    0x02                 //  RCS submode in CAM
#define  RMODE     0x03                 //  RCS operation mode

#define  CAMOD_NO  0x0000               //  no CAM operation
#define  CAMOD_GO  0xffff               //  start CAM with ACAM (move/ac/el)
#define  CAMOD_FF  0x000f               //  goal ahead, moving forward
#define  CAMOD_FB  0x00f0               //  goal ahead, moving backward
#define  CAMOD_BF  0x0f00               //  goal back, moving forward
#define  CAMOD_BB  0xf000               //  goal back, moving backward

#define  MODE_NOP  0                    //  no operation
#define  MODE_POS  1                    //  position control
#define  MODE_VEL  2                    //  velocity control (reserved)
#define  MODE_TOR  3                    //  torque control (reserved)

#define  MMFLG     0x04                 //  minmax finder flag
#define  MM_NOP    0x0000               //  not in minmax mode
#define  MM_MIN    0xf000               //  mining flag for MMF
#define  MM_MAX    0x000f               //  maxing flag for MMF
#define  MM_GO_CEN 0x03c0               //  go-to-cen flag for MMF
#define  MM_GO_MIN 0x0c00               //  go-to-min flag for MMF
#define  MM_GO_MAX 0x0030               //  go-to-max flag for MMF

#define  MMEMG     0x05                 //  minmax error magnitude

#define  REP_INT   0x06                 //  report interval (of Dt)
#define  REP_CNT   0x07                 //  report count (tmp)
#define  REP_ACT   0x08                 //  report count in action
#define  REP_TIC   0x09                 //  report interval tick

//
//  initialize variables

void  VAR_init ()
{
    int  n;
    fix  erpc8, lpfa;

    //  clear F/W blocks
    for (n = 0; n < NN; n++) {
        int  index;

        //  clear fix block
        for (index = 0; index < NF; index++) F[index][n] = 0;
        //  clear word block
        for (index = 0; index < NW; index++) W[index][n] = 0;
    }

    //  set encoder resolution and lpf parameters (according to ADDR)
    if (ADDR & 0x10) {                  //  0x10..0x1e -> 256ppr/1000Hz
        erpc8 = ERPC8_256;
        lpfa  = LPFA_256;
    }
    else if (ADDR & 0x08) {             //  0x08..0x0f -> 200ppr/1000Hz
        erpc8 = ERPC8_200; 
        lpfa  = LPFA_200;
    }
    else {                              //  0x00..0x07 -> 16ppr/100Hz
        erpc8 = ERPC8_16;
        lpfa  = LPFA_16;
    }

    //  init F/W blocks (for non-zero init value)
    for (n = 0; n < NN; n++) {
        //  F
        F[KP][n]    = KP_DEF;
        F[KI][n]    = KI_DEF;
        F[KD][n]    = KD_DEF;
        F[ACAM0][n] = ACAM0_DEF;
        F[VCAM0][n] = VCAM0_DEF;
        F[MIN][n]   = MIN_DEF;
        F[MAX][n]   = MAX_DEF;
        F[PMAR][n]  = PMAR_DEF;
        F[ERPC8][n] = erpc8;
        F[LPFA][n]  = lpfa;
        F[LPFD][n]  = FIX_sub(0x00010000, lpfa);
        F[VMP][n]   = VMP_DEF;
        F[VMAG][n]  = FIX_div(0x7fffffff, VMP_DEF);
        F[RXPOS][n] = RXPOS_DEF;
        F[RACAM][n] = RACAM_DEF;
        F[RVCAM][n] = RVCAM_DEF;
        //  W
        W[MODE][n]  = MODE_POS;
        W[RMODE][n] = MODE_POS;
    }
}

////////////////////////////////////////////////////////////////////////
//
//  ACT: actuation process

void  ACT_do (void)
{
    fix  ACT_enc(uchar);                //  encoder read in rad
    void  PAC_report(int, fix),         //  position report for "report"
          MMF_do(uchar),                //  minmax finder module
          CAM_do(uchar, uchar, uchar),  //  trapezoid control module
          PID_do(uchar);                //  PID control module

    static uchar  n = 0;                //  axis number {0,1,2,3}
    fix  pos, vel;                      //  current pos, vel (tmp)

    //  update POS and VEL
    pos = ACT_enc(n);
    F[POS][n] = pos;
    vel = FIX_mul(FIX_sub(pos, F[POLD][n]), FQf);
    F[POLD][n] = pos;

    //  low-pass filter for velocity
    //      (needed for stabilizing the differentiation)
    //      LPF = (1.0 - LPFA) * VEL + LPFA * VEL
    F[VEL][n] = FIX_add(FIX_mul(F[LPFD][n], F[VEL][n]), 
                        FIX_mul(F[LPFA][n], vel) );

    //  report (if needed)
    if (W[REP_ACT][n]) {
        if (W[REP_TIC][n] == 0) {
            PAC_report(n, pos);             //  report POS
            W[REP_ACT][n]--;                //  decrement counter
            W[REP_TIC][n] = W[REP_INT][n];  //  reset interval
        }
        W[REP_TIC][n]--;
    }

    //  control the n-th motor
    switch (W[MODE][n]) {
        case MODE_NOP:  //  no-operator (and raw mode)
            break;
        case MODE_POS:  //  position control
            //  if minmax then
            if (W[MMFLG][n]) MMF_do(n);
            //  if CAM then
            if (W[CAMOD][n]) CAM_do(n, FO_MOT, WO_MOT);
            //  PID control
            PID_do(n);
            break;
        case MODE_VEL:  //  velocity control (experimental)
            PWM_set(n, FIX_mul(F[XVEL][n], F[VMAG][n]) >> 16);
            break;
        case MODE_TOR:  //  torque control (experimental)
            break;
    }

    //  control the n-th RCS
    switch (W[RMODE][n]) {
        case MODE_NOP:  //  no-operator (and raw mode)
            break;
        case MODE_POS:  //  position control
            //  if cam then
            if (W[RCAMOD][n]) CAM_do(n, FO_RCS, WO_RCS);
            //  actuate the RCS
            RCS_set(n, F[RXPOS][n] >> 16);
            break;
    }

    //  update counters
    n++;
    if (n == NN) {
        ushort  tor0, tor1, tor2, tor3;

        //  reset/update the counters
        n = 0;

        //  sense torque/current
        //SEN_gets(&tor0, &tor1, &tor2, &tor3);
        //F[TOR][0] = tor0 << 16;
        //F[TOR][1] = tor1 << 16;
        //F[TOR][2] = tor2 << 16;
        //F[TOR][3] = tor3 << 16;

        //  clear watch-dog timer
        WDT_reset();
    }
}

//  fix  ACT_enc ()
//    returns encoder position in rad (radian)
//    in fixed point s1616 format;

fix  ACT_enc (uchar n)
{
    //  ENC_get(n): Encoder count in 24bit integer
    //  EPRC8[n]  : Encoder resolution (rad/count; s0824)
    //      --------------- s1616 -----------------
    //              ----- s2408 -----  -- s0824 --
    return  FIX_mul((ENC_get(n) << 8), F[ERPC8][n]);
}

////////////////////////////////////////////////////////////////////////
//
//  PID: controller

void  PID_do (uchar n)
{
    fix  xposf,                         //  goal position (after limitter)
         erf,                           //  error (s1616)
         vpf,                           //  output of proportional control
         vdf,                           //  output of differential control
         vof;                           //  final output

    //  limit range of motion in [MIN, MAX]
    xposf = F[XPOS][n];
    if (xposf < F[MIN][n]) xposf = F[MIN][n];
    if (xposf > F[MAX][n]) xposf = F[MAX][n];

    //  calculate error
    //      erf = xposf - POSf
    erf = FIX_sub(xposf, F[POS][n]);

    //  proportional control
    //      vpf = KPf * erf
    vpf = FIX_mul(F[KP][n], erf);

    //  integral control
    //      VINT += KIf * DTf * ERf
    F[VINT][n] = FIX_add(F[VINT][n],    //  --- s2408 --- -s0824-
                         FIX_mul(FIX_mul((F[KI][n] >> 8), DTf8), erf));
    if (F[VINT][n] >  F[VMP][n]) F[VINT][n] =  F[VMP][n];
    if (F[VINT][n] < -F[VMP][n]) F[VINT][n] = -F[VMP][n];

    //  differential control (position-based version)
    //      vdf = KDf * (POLD - POS) / DTf  --- (DTf: DeltaT=0.001s)
    //          = KDf * (POLD - POS) * FQf  --- (FQf: Freq=1/DeltaT)
    //          = - KDf * VEL
    vdf = -FIX_mul(F[KD][n], F[VEL][n]);

    //  sum up the outputs
    //      vof = (vpf + VINT + vdf);
    vof = FIX_add(vpf, FIX_add(F[VINT][n], vdf));
    if (vof >  F[VMP][n]) vof =  F[VMP][n];
    if (vof < -F[VMP][n]) vof = -F[VMP][n];

    //  power meter for debugging
    if (vof >= 0)
        LED_op(0x0f, (vof >> 16) & 0x0f);
    else
        LED_op(0x0f, ((-vof) >> 16) & 0x0f);

    //  PWM output (s1616 -> short)
    PWM_set(n, FIX_mul(vof, F[VMAG][n]) >> 16);
}

////////////////////////////////////////////////////////////////////////
//
//  CAM: constant acceleration mode (MOT/RCS shared version)
//      for MOT (motor), provide FO_MOT and WO_MOT for fo and wo;
//      for RCS (servo), provide FO_RCS and WO_RCS for fo and wo;
//      (these values will give bias for var-block index)

void  CAM_do (uchar n, uchar fo, uchar wo)
{
    fix  acam, vcam, xvel;

    acam = FIX_mul(FIX_mul(F[ACAM+fo][n] >> 8, DTf8) >> 8, DTf8);
    vcam = FIX_mul(F[VCAM+fo][n] >> 8, DTf8);
    xvel = F[XVEL+fo][n];

    //  ACAM0 should be more than 15.26 (=1000*1000/65536)
    //  (in any case, acam should be more than 0x00000001)
    if (acam <= 0) acam = 0x00000001;

    //  CAM operation
    if (xvel > acam) {
        //  I'm moving forward
        if (F[XPOS+fo][n] > F[PCAM+fo][n]) {
            //  and the goal is behind!
            W[CAMOD+wo][n] = CAMOD_BF;

            //  decrease speed, anyway
            xvel = FIX_sub(xvel, acam);
            if (xvel < -vcam)
                xvel = -vcam;
        }
        else {
            int  vabsf, pcrif;

            //  the goal is ahead
            //  (or I'm passing above the goal forward)
            W[CAMOD+wo][n] = CAMOD_FF;

            //  compute the critical point for braking
            vabsf = xvel;
            pcrif = FIX_sub(F[PCAM+fo][n], 
                            FIX_div(FIX_mul(vabsf, FIX_add(vabsf, acam)), 
                                    FIX_add(acam, acam) ));

            //  If I have passed (or am passing) the point, 
            //      then decrease speed
            //      otherwise increase speed
            if (F[XPOS+fo][n] >= FIX_sub(pcrif, vabsf)) {
                xvel = FIX_sub(xvel, acam);
                if (xvel < -vcam)
                    xvel = -vcam;
            }
            else {
                xvel = FIX_add(xvel, acam);
	        if (xvel >  vcam)
                    xvel =  vcam;
            }
        }
    }
    else if (xvel < -acam) {
        //  I'm moving backward
        if (F[XPOS+fo][n] < F[PCAM+fo][n]) {
            //  and the goal is ahead!
            W[CAMOD+wo][n] = CAMOD_FB;

            //  increase speed, anyway
            xvel = FIX_add(xvel, acam);
            if (xvel >  vcam)
                xvel =  vcam;
        }
        else {
            int  vabsf, pcrif;

            //  the goal is behind
            //  (or I'm passing above the goal backward)
            W[CAMOD+wo][n] = CAMOD_BB;

            //  compute the critical point for braking
            vabsf = -xvel;
            pcrif = FIX_add(F[PCAM+fo][n], 
                            FIX_div(FIX_mul(vabsf, FIX_add(vabsf, acam)), 
                                    FIX_add(acam, acam) ));

            //  If I have passed (or am passing) the point, 
            //      then increase speed
            //      otherwise decrease speed
            if (F[XPOS+fo][n] <= FIX_add(pcrif, vabsf)) {
                xvel = FIX_add(xvel, acam);
	        if (xvel >  vcam)
                    xvel =  vcam;
            }
            else {
                xvel = FIX_sub(xvel, acam);
                if (xvel < -vcam) 
                    xvel = -vcam;
            }
        }
    }
    else {
        //  I'm almost stand still (VNOW is almost zero)
        if (F[XPOS+fo][n] < FIX_sub(F[PCAM+fo][n], acam)) {
            //  the goal is ahead
            W[CAMOD+wo][n] = CAMOD_FF;

            //  increase speed to move forward
            xvel = FIX_add(xvel, acam);
            if (xvel >  vcam) 
                xvel =  vcam;
        }
        else if (FIX_add(F[PCAM+fo][n], acam) < F[XPOS+fo][n]) {
            //  the goal is behind
            W[CAMOD+wo][n] = CAMOD_BB;

            //  decrease speed to move backward
            xvel = FIX_sub(xvel, acam);
            if (xvel < -vcam) 
                xvel = -vcam;
        }
        else {
            //  I'm almost at the goal
            //  (force to stop moving at the goal)
            F[XPOS+fo][n] = F[PCAM+fo][n];
            xvel = 0;
            W[CAMOD+wo][n] = CAMOD_NO;
            return;
        }
    }

    //  finally update the goal position and goal velocity
    F[XPOS+fo][n] = FIX_add(F[XPOS+fo][n], xvel);
    F[XVEL+fo][n] = xvel;
}

////////////////////////////////////////////////////////////////////////
//
//  MMF: minmax finder

void  MMF_do (uchar n)
{
    fix  mvel, merr;

    //  indicate MMF-ing
    LED_op(0x10, 0x10);

    //  minmax velocity (VCAM0/FQf)
    //  (for different velocity, change setvel before do_min/max/etc)
    mvel = FIX_mul(F[VCAM0][n] >> 8, DTf8);

    //  error for collision detection
    //  (ushort MMEMG [rad])
    merr = FIX_mul(mvel, W[MMEMG][n] << 16);

    if (W[MMFLG][n] & MM_MIN) {
        //  find min end

        //  if in CAM, stop it (otherwise XPOS would get stuck)
        W[CAMOD][n] = CAMOD_NO;

        //  check collision with min end
        if (-FIX_sub(F[XPOS][n], F[POS][n]) > merr) {
            //  collision!!
            //  set MIN at POS+margin
            F[MIN][n] = FIX_add(F[POS][n], F[PMAR][n]);
            //  stop motor at MIN
            F[XPOS][n] = F[MIN][n];
            //  clear MM_MIN flag
            W[MMFLG][n] &= ~(MM_MIN);
            LED_op(0x10, 0x00);
        }
        else {
            //  no collision, so go backward
            F[XPOS][n] = FIX_sub(F[XPOS][n], mvel);
        }            
    }
    else if (W[MMFLG][n] & MM_MAX) {
        //  find max end

        //  if in CAM, stop it (otherwise XPOS would get stuck)
        W[CAMOD][n] = CAMOD_NO;

        //  check collision with max end
        if (FIX_sub(F[XPOS][n], F[POS][n]) > merr) {
            //  collision!!
            //  set MIN at POS-margin
            F[MAX][n] = FIX_sub(F[POS][n], F[PMAR][n]);
            //  stop motor at MAX
            F[XPOS][n] = F[MAX][n];
            //  clear MM_MAX flag
            W[MMFLG][n] &= ~(MM_MAX);
            LED_op(0x10, 0x00);
        }
        else {
            //  no collision, so go forward
            F[XPOS][n] = FIX_add(F[XPOS][n], mvel);
        }            
    }
    else if (W[MMFLG][n] & (MM_GO_CEN | MM_GO_MAX | MM_GO_MIN)) {
        fix  xposf, diff;

        //  set goal position
        if (W[MMFLG][n] & MM_GO_MAX)
            xposf = F[MAX][n];
        else if (W[MMFLG][n] & MM_GO_MIN)
            xposf = F[MIN][n];
        else //  center
            xposf = FIX_mul(FIX_add(F[MIN][n], F[MAX][n]), 0x00008000);

        //  forward/backword/terminte
        diff = FIX_sub(xposf, F[XPOS][n]);
        if (diff > mvel) {
            //  move forward
            F[XPOS][n] = FIX_add(F[XPOS][n], mvel);
        }
        else if (diff < -mvel) {
            //  move backward
            F[XPOS][n] = FIX_sub(F[XPOS][n], mvel);
        }
        else {
            //  terminate
            fix  offset;

            //  save offset
            offset = xposf;
            //  zero (make current position "zero")
            F[XPOS][n] = 0;
            F[POS][n]  = 0;
            F[POLD][n] = 0;
            F[VEL][n]  = 0;
            F[VINT][n] = 0;
            ENC_set(n, 0);
            //  move MIN/MAX
            F[MIN][n] = FIX_sub(F[MIN][n], offset);
            F[MAX][n] = FIX_sub(F[MAX][n], offset);
            //  clear the flag
            W[MMFLG][n] = MM_NOP;
            LED_op(0x10, 0x00);
        }
    }
}

////////////////////////////////////////////////////////////////////////
//
//  PAC: packet translator

void  PAC_to_NIQ (uchar *packet, ushort *n, ushort *inst, uint *quad)
{
    uchar  msbs;
    uchar  data1, data2, data3, data4;

    //  motor number (N)
    *n = packet[0] & 0x03;

    //  inst (I) from upper 3 bits of packet[1]
    //  (lower 4 bits are msbs for arg(g1-4))
    *inst = packet[1] >> 4;
    msbs = packet[1] & 0x0f;

    //  quad (Q) = data1..4
    data1 = packet[2] | ((msbs & 0x08)? 0x80: 0);
    data2 = packet[3] | ((msbs & 0x04)? 0x80: 0);
    data3 = packet[4] | ((msbs & 0x02)? 0x80: 0);
    data4 = packet[5] | ((msbs & 0x01)? 0x80: 0);
    *quad = (data1 << 24) | (data2 << 16) | (data3 << 8) | data4;
}

void  PAC_from_NIQ (uchar *packet, ushort n, ushort inst, uint quad)
{
    packet[0] = (ADDR << 2) | (n & 0x03);
    packet[1] = (inst & 0x07) << 4
              | ((quad & 0x80000000)? 0x08: 0)
              | ((quad & 0x00800000)? 0x04: 0)
              | ((quad & 0x00008000)? 0x02: 0)
              | ((quad & 0x00000080)? 0x01: 0);
    packet[2] = (quad & 0x7f000000) >> 24;
    packet[3] = (quad & 0x007f0000) >> 16;
    packet[4] = (quad & 0x00007f00) >>  8;
    packet[5] = (quad & 0x0000007f);
}

//  PAC_do: packet interpreter

ushort  Low16;                          //  lower 16bits for fix

void  PAC_do (uchar *packet)
{
    ushort  n, inst;
    uint    quad;
    uchar   r_packet[PACLEN];
    void    PAC_report_trigger(int);

    PAC_to_NIQ(packet, &n, &inst, &quad);
    if (inst == 0) {
        //  move: move to pos in CAM with preset acc
        F[ACAM][n] = F[ACAM0][n];
        F[VCAM][n] = F[VCAM0][n];
        F[PCAM][n]  = (fix) quad;
        W[CAMOD][n] = CAMOD_GO;

        //  if needed, trigger reporting
        if (W[REP_CNT][n]) PAC_report_trigger(n);
    }
    else if (inst == 1) {
        fix  arg;

        arg = (fix) quad;
        if (arg < 0) {
            //  movac: move to pos in CAM with -acc
            //    (arg = acc:n16,pos:s16)
            F[ACAM][n] = - (arg & 0xffff0000);
            F[VCAM][n] = F[VCAM0][n];
            F[PCAM][n]  = (fix) (arg << 16);
            W[CAMOD][n] = CAMOD_GO;

            //  if needed, trigger reporting
            if (W[REP_CNT][n]) PAC_report_trigger(n);
        }
        else {
            //  setacc: acceleration for move
            F[ACAM0][n] = arg;
        }
    }
    else if (inst == 2) {
        fix  arg;

        arg = (fix) quad;
        if (arg < 0) {
            //  movel: move to pos in CAM with -vel
            //    (arg = vel:n16,pos:s16)
            F[ACAM][n] = F[ACAM0][n];
            F[VCAM][n] = - (arg & 0xffff0000);
            F[PCAM][n]  = (fix) (arg << 16);
            W[CAMOD][n] = CAMOD_GO;

            //  if needed, trigger reporting
            if (W[REP_CNT][n]) PAC_report_trigger(n);
        }
        else {
            //  setvel: velocity for move
            F[VCAM0][n] = arg;
        }
    }
    else if (inst == 3) {
        //  setpos: ballistic motion

        //  cancel CAM
        W[CAMOD][n] = CAMOD_NO;
        //  set to XPOS
        F[XPOS][n] = (fix) quad;

        //  if needed, trigger reporting
        if (W[REP_CNT][n]) PAC_report_trigger(n);
    }
    else if (inst == 5) {
        //  getacc: current acceleration
        quad = (uint) F[ACAM][n];
        PAC_from_NIQ(r_packet, n, inst, quad);
        SCI_transmit(r_packet);
    }
    else if (inst == 6) {
        //  getvel: current velocity
        quad = (uint) F[VEL][n];
        PAC_from_NIQ(r_packet, n, inst, quad);
        SCI_transmit(r_packet);
    }
    else if (inst == 7) {
        //  getpos: current position
        quad = (uint) F[POS][n];
        PAC_from_NIQ(r_packet, n, inst, quad);
        SCI_transmit(r_packet);
    }
    else if (inst == 4) {
        ushort  jnst, index, word;

        //  sub-command
        jnst  = quad >> 24;
        index = (quad & 0x00ff0000) >> 16;
        word  = quad & 0x0000ffff;

        if (jnst == 0x00) {
            //  set_fix
            if (index == 0xff)
                Low16 = word;
            else if (index < NF) {
                F[index][n] = (fix) ((word << 16) | Low16);

                //  auto set variables (VMAG, LPFD)
                if (index == VMP) {
                    //  VMAG = 32768 / VMP;
                    F[VMAG][n] = FIX_div(0x7fffffff, F[VMP][n]);
                }
                else if (index == LPFA) {
                    //  LPFD = 1.0 - LPFA
                    F[LPFD][n] = FIX_sub(0x00010000, F[LPFA][n]);
                }
	    }
        }
        else if (jnst == 0x01) {
            //  set_word
            if (index < NW)
                W[index][n] = word;
        }
        else if (jnst == 0x70) {
            switch (index) {
                case 0x00:
                    //  move_rcs
                    F[RPCAM][n]  = (fix) (word << 16);
                    W[RCAMOD][n] = CAMOD_GO;
                    break;
                case 0x01:
                    //  setacc_rcs
                    F[RACAM][n] = (fix) (word << 16);
                    break;
                case 0x02:
                    //  setvel_rcs
                    F[RVCAM][n] = (fix) (word << 16);
                    break;
                case 0x03:
                    //  setpos_rcs
                    W[RCAMOD][n] = CAMOD_NO;
                    F[RXPOS][n]  = (fix) (word << 16);
                    break;
            }
        }
        else if (jnst == 0x7f) {
            switch (index) {
                case 0x00:  //  set_zero
                    W[CAMOD][n] = CAMOD_NO;
                    F[XPOS][n]  = 0;
                    F[POS][n]   = 0;
                    F[POLD][n]  = 0;
                    F[VEL][n]   = 0;
                    F[VINT][n]  = 0;
                    ENC_set(n, 0);
                    break;
                case 0x01:  //  set_mode
                    W[MODE][n] = word;
                    break;
                case 0x02:  //  set_camod
                    W[CAMOD][n] = word;
                    break;
                case 0x04:  //  go_cen
                    W[MMFLG][n] = MM_GO_CEN;
                    break;
                case 0x05:  //  do_max
                    W[MMFLG][n] = MM_MAX;
                    W[MMEMG][n] = word;
                    F[MAX][n] = MAX_DEF;
                    break;
                case 0x06:  //  do_min
                    W[MMFLG][n] = MM_MIN;
                    W[MMEMG][n] = word;
                    F[MIN][n] = MIN_DEF;
                    break;
                case 0x07:  //  do_minmax
                    W[MMFLG][n] = (MM_MIN | MM_MAX);
                    W[MMEMG][n] = word;
                    F[MIN][n] = MIN_DEF;
                    F[MAX][n] = MAX_DEF;
                    break;
                case 0x08:  //  do_minmaxcen
                    W[MMFLG][n] = (MM_MIN | MM_MAX | MM_GO_CEN);
                    W[MMEMG][n] = word;
                    F[MIN][n] = MIN_DEF;
                    F[MAX][n] = MAX_DEF;
                    break;
                case 0x09:  //  do_minmaxmax
                    W[MMFLG][n] = (MM_MIN | MM_MAX | MM_GO_MAX);
                    W[MMEMG][n] = word;
                    F[MIN][n] = MIN_DEF;
                    F[MAX][n] = MAX_DEF;
                    break;
                case 0x0a:  //  do_minmaxmin
                    W[MMFLG][n] = (MM_MIN | MM_MAX | MM_GO_MIN);
                    W[MMEMG][n] = word;
                    F[MIN][n] = MIN_DEF;
                    F[MAX][n] = MAX_DEF;
                    break;
                case 0xff:  //  reset
                    VAR_init();
                    for (n = 0; n < NN; n++)
                        ENC_set(n, 0);
                    break;
            }
        }
        else if (jnst == 0x80) {
            //  get_fix F[index][n]
            if (index == 0xff)
                quad = (quad & 0xffff0000) | Low16;
            else if (index < NF) {
                uint  tmp;

                tmp = (uint) F[index][n];
                Low16 = tmp & 0xffff;
                quad = (quad & 0xffff0000) | (tmp >> 16);
            }
            else
                quad = 0xffffffff;      //  to indicate error
            PAC_from_NIQ(r_packet, n, inst, quad);
            SCI_transmit(r_packet);
        }
        else if (jnst == 0x81) {
            //  get_word W[index][n]
            if (index < NW)
                quad = (quad & 0xffff0000) | W[index][n];
            else
                quad = 0xffffffff;      //  to indicate error
            PAC_from_NIQ(r_packet, n, inst, quad);
            SCI_transmit(r_packet);
        }
        else if (jnst == 0x82) {
            //  get_version
            quad = (quad & 0xffff0000) | VER;
            PAC_from_NIQ(r_packet, n, inst, quad);
            SCI_transmit(r_packet);
        }
        else if (jnst == 0xf0) {
            switch (index) {
                case 0x05:
                    //  getacc_rcs
                    word = (ushort) (F[RACAM][n] >> 16);
                    quad = (quad & 0xffff0000) | word;
                    break;
                case 0x06:
                    //  getvel_rcs
                    word = (ushort) (F[RVCAM][n] >> 16);
                    quad = (quad & 0xffff0000) | word;
                    break;
                case 0x07:
                    //  getpos_rcs
                    if (W[RCAMOD][n])
                        word = (ushort) (F[RPCAM][n] >> 16);
                    else
                        word = (ushort) (F[RXPOS][n] >> 16);
                    quad = (quad & 0xffff0000) | word;
                    break;
                default:
                    quad = 0xffffffff;
            }
            PAC_from_NIQ(r_packet, n, inst, quad);
            SCI_transmit(r_packet);
        }
        else if (jnst == 0xff) {
            //  get_report
            W[REP_INT][n] = index;      //  interval (# of control loops)
            W[REP_CNT][n] = word;       //  report counts
        }
        else if (jnst >= 0x80) {
            //  unknown query (return "error" packet)
            quad = 0xffffffff;
            PAC_from_NIQ(r_packet, n, inst, quad);
            SCI_transmit(r_packet);
        }
    }
}

void  PAC_report_trigger (int n)
{
    W[REP_ACT][n] = W[REP_CNT][n];
    W[REP_CNT][n] = 0;
    W[REP_TIC][n] = 0;
}

void  PAC_report (int n, fix pos)
{
    uchar  r_packet[PACLEN];

    PAC_from_NIQ(r_packet, n, 7, pos);  //  report packet (I=7)
    SCI_transmit(r_packet);
}

////////////////////////////////////////////////////////////////////////