/****************************************************************************/
/* Copyright (c) 2000 MBARI.                                                */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : SV203B Servo Motot Controller - Function Definitions          */
/* Filename : sv203.c                                                       */
/* Author   : Andrew Pearce                                                 */
/* Project  : Dorado AUV Tailcone                                           */
/* Version  : 1.0                                                           */
/* Created  : 01/19/00                                                      */
/* Modified : 01/19/00                                                      */
/* Archived :                                                               */
/****************************************************************************/

#pragma model(196)
#pragma nosignedchar

#include "C:/C96/INCLUDE/string.h"      /* string function library          */

#include "const.h"                      /* Misc. constants - TRUE/FALSE etc */
#include "types.h"                      /* MBARI style guide declarations   */
#include "microasm.h"                   /* Assembly language functions      */
#include "applic.h"                     /* Microcontroller applications     */
#include "malloc.h"                     /* Malloc support routines          */
#include "strlib.h"

#include "microdef.h"                   /* Microcontroller definitions      */
#include "microlib.h"                   /* Microcontroller Library decls.   */

#include "timer.h"                      /* Timer library definitions        */
#include "ring.h"                       /* Ring buffer support routines     */
#include "syslib.h"                     /* Board Support library decls.     */

#include "ibc_card.h"                   /* IBC Board Types and Addresses    */
#include "ibc_cmd.h"                    /* Core IBC application definitions */
#include "ibc.h"                        /* IBC Function Library Definitions */

#include "quad_serial.h"                /* Quad Serial Board Definitions    */
#include "scc2698.h"                    /* Signetics SCC2698 UART Definition*/

#include "sv203.h"

/****************************************************************************/

    Int16
sv203OpenSerialChan(sv203Struct *sv, Byte *boardAddr, Nat16 chan,
    IBC_BoardEntry *IBC_CardTable[], Int16 cardCount )
{
    Int16 serialCard;           /* Serial Card index into IBC Card Table    */

    sv->serialChan.boardEntry = (serialBoardEntry*) NULL;

                                /* Find card index of serial board in table */
    if ( (serialCard = ibcBoardTableIndex(IBC_CardTable, cardCount,
         boardAddr, QUAD_SERIAL) ) == ERROR)
        return (ERROR);

    sv->serialChan.boardEntry = (serialBoardEntry*) IBC_CardTable[serialCard];
    sv->serialChan.channel    = chan;

    serialBoardOpenChan(sv->serialChan.boardEntry, chan);
                                 /* Set Baud Rate to 9600 baud              */
                                 /* Set line format 8 data,  1 stop         */
                                 /* 2 = NO PARITY, Handshake OFF, RS485 mode*/
    sccSetLineFormat(sv->serialChan.boardEntry, chan,
             9600, 8, 1, 2, HANDSHAKE_OFF, QS_RS485_MODE);

     sv->lastMoveAbs = 0;

     return(OK);
} /* openStp100SerialChan() */

    Int16
sv203WriteData(sv203Struct *sv, char *cmd)
{
  char term[2];

  if (sv->serialChan.boardEntry == NULL)
          return(ERROR);

  taskDelay(SV203_CMD_DELAY);

  serialBoardWrite(sv->serialChan.boardEntry, sv->serialChan.channel,
                  cmd, strlen(cmd));

  term[0] = SV203_TERM;
  term[1] = 0;

  serialBoardWrite(sv->serialChan.boardEntry, sv->serialChan.channel,
                  term, strlen(term));
} /* sv203WriteData() */


    Int16
sv203ReadData(sv203Struct *sv, Byte *buffer, Int16 maxLen)
{
    Int16  status;

    if (sv->serialChan.boardEntry == NULL)
      return(ERROR);

    while (serialBoardTermRecvd(sv->serialChan.boardEntry,
            sv->serialChan.channel) )
    {
        if ( (status = serialBoardRead(sv->serialChan.boardEntry,
            sv->serialChan.channel, buffer, maxLen)) <= 0)

            serialBoardRxFlush(sv->serialChan.boardEntry,
                sv->serialChan.channel );

        return(status);
    } /* while */

    return (status);
} /* sv203ReadData() */

    Int16
sv203BoardSelect(sv203Struct *sv, Int16 board)
{
  char cmd[10] = "BD";

  if (board > 255)              /* Invalid board Address */
    return (ERROR);

  itoa(cmd + 2, (Nat32) board, 0, 10, ' ', UNSIGNED);
  sv203WriteData(sv, cmd);

  return (sv203ReadData(sv, cmd, 10));
} /* sv203BoardSelect() */

    Int16
sv203ServoSelect(sv203Struct *sv, Int16 servo)
{
  char cmd[10] = "SV";

  if ((servo < 1) || (servo > 8))       /* Invalid servo Address */
    return (ERROR);

  itoa(cmd + 2, (Nat32) servo, 1, 10, ' ', UNSIGNED);
  sv203WriteData(sv, cmd);

  return (sv203ReadData(sv, cmd, 10));
} /* sv203ServoSelect() */

    Int16
sv203ReadFirwareID(sv203Struct *sv, char *firmware)
{
  char cmd[10] = "V?";

  sv203WriteData(sv, cmd);

  return (sv203ReadData(sv, firmware, 10));
} /* sv203ReadFirwareID() */


    Int16
sv203ReadAtoD(sv203Struct *sv, Int16 chan, Byte *value)
{
  char cmd[10] = "AD";

  if ((chan < 1) || (chan > 5))
    return (ERROR);

  itoa(cmd + 2, (Nat32) chan, 1, 10, ' ', UNSIGNED);
  sv203WriteData(sv, cmd);

  sv203ReadData(sv, cmd, 10);
  *value = atoi(cmd);

  return (OK);
} /* sv203ReadAtoD() */


    Int16
sv203MoveAbsolute(sv203Struct *sv, Byte position)
{
  char cmd[10] = "M";
  Byte delta;

  if (sv->lastMoveAbs == 0)
      sv->lastMoveAbs = position;

  delta = abs(sv->lastMoveAbs - position);
  if (delta > 5)
  {
     if (position > sv->lastMoveAbs)
          position = sv->lastMoveAbs + 5;
     else
          position = sv->lastMoveAbs - 5;
  };

  sv->lastMoveAbs = position;

  itoa(cmd + 1, (Nat32) position, 0, 10, ' ', UNSIGNED);
  sv203WriteData(sv, cmd);

  return (sv203ReadData(sv, cmd, 10));
} /* sv203MoveAbsolute() */