//////////////////////////////////////////////////////////////////////////////
//
//  c-movado-link: Movado interface in C (Linux/Mac/Windows)
//      version 1.2 (October 9, 2009)
//
//  (C) Hideki Kozima (xkozima@myu.ac.jp), subject to GPLv2
//

#include "c-movado.c"

//////////////////////////////////////////////////////////////////////////////
//
//  movado_move/movac/movel (trapezoid motion control)
//
//  movado_move:  move to pos [rad]
//  movado_movac: move to pos [rad] with (acc > 0.0) [rad/s/s]
//  movado_movel: move to pos [rad] with (vel > 0.0) [rad/s]

void  movado_move (int addr, int n, double pos)
{
    int  quad;

    quad = pos * 65536;
    pac_do(addr, n, 0, &quad);
}

void  movado_movac (int addr, int n, double pos, double acc)
{
    int  ipos, iacc;
    int  quad;

    ipos = pos * 65536;
    iacc = acc * 65536;
    if (iacc <= 0) return;
    quad = ((-iacc) & 0xffff0000) | (ipos >> 16);
    pac_do(addr, n, 1, &quad);
}

void  movado_movel (int addr, int n, double pos, double vel)
{
    int  ipos, ivel;
    int  quad;

    ipos = pos * 65536;
    ivel = vel * 65536;
    if (ivel <= 0) return;
    quad = ((-ivel) & 0xffff0000) | (ipos >> 16);
    pac_do(addr, n, 2, &quad);
}

//////////////////////////////////////////////////////////////////////////////
//
//  movado_setacc/setvel/setpos
//
//  movado_setacc: set acc (> 0.0) [rad/s/s]
//  movado_setvel: set vel (> 0.0) [rad/s]
//  movado_setpos: set pos [rad] (ballistic move)

void  movado_setacc (int addr, int n, double acc)
{
    int  quad;

    quad = acc * 65536;
    if (quad < 0) return;
    pac_do(addr, n, 1, &quad);
}

void  movado_setvel (int addr, int n, double vel)
{
    int  quad;

    quad = vel * 65536;
    if (quad < 0) return;
    pac_do(addr, n, 2, &quad);
}

void  movado_setpos (int addr, int n, double pos)
{
    int  quad;

    quad = pos * 65536;
    if (quad < 0) return;
    pac_do(addr, n, 3, &quad);
}

//////////////////////////////////////////////////////////////////////////////
//
//  movado_getacc/getvel/getpos
//
//  movado_getacc: get acc [rad/s/s] (preset acc)
//  movado_getvel: get vel [rad/s]   (current vel)
//  movado_getpos: get pos [rad]     (current pos)

double  movado_getacc (int addr, int n)
{
    int  quad;
    double  acc;

    pac_do(addr, n, 5, &quad);
    acc = quad / 65536.0;

    return acc;
}

double  movado_getvel (int addr, int n)
{
    int  quad;
    double  vel;

    pac_do(addr, n, 6, &quad);
    vel = quad / 65536.0;

    return vel;
}

double  movado_getpos (int addr, int n)
{
    int  quad;
    double  pos;

    pac_do(addr, n, 7, &quad);
    pos = quad / 65536.0;

    return pos;
}

//////////////////////////////////////////////////////////////////////////////
//
//  movado_sub

int  movado_sub (int addr, int n, int jnst, int index, int word)
{
    int  quad;

    quad = ((jnst&0xff) << 24) | ((index&0xff) << 16) | (word&0xffff); 
    pac_do(addr, n, 4, &quad);

    return  quad & 0xffff;
}

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