/****************************************************************************/
/* 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    */
  Nat16 termChar = SV203_TERM;

    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);

#if 0
    sccSetTermChar(sv->serialChan.boardEntry, chan, termChar);
#endif
     sv->lastMoveAbs = 0;

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

    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));

 return(OK);

} /* sv203WriteData() */

   Int16
sv203ReadData(sv203Struct *sv, Byte *buffer, Int16 maxLen, Nat16 timeoutticks )
{
    Int16  status;
    Nat16  starttic = read_sysclock();
    
    if (sv->serialChan.boardEntry == NULL)
      return(ERROR);

    while (serialBoardTermRecvd(sv->serialChan.boardEntry,
            sv->serialChan.channel) == 0)
    {
    if( (read_sysclock() - starttic) >= timeoutticks)
	  return (ERROR);
    }
    
    while (serialBoardTermRecvd(sv->serialChan.boardEntry,
            sv->serialChan.channel))
    {
     #if 0
	 status = serialBoardRead(sv->serialChan.boardEntry,
               sv->serialChan.channel, buffer, maxLen);
     #endif
         if ( (status = serialBoardRead(sv->serialChan.boardEntry,
            sv->serialChan.channel, buffer, maxLen)) <= 0)

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

    }
    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, 0));
} /* 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, 0));
} /* sv203ServoSelect() */

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

  sv203WriteData(sv, cmd);

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

    Int16
sv203ReadAtoD(sv203Struct *sv, Int16 chan)
{
  char cmd[10] = "AD";
  char rcv[10];
  Int16 value;
  Nat16 timeout = 10;
  Int16 bytes = 0;

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

  itoa(cmd + 2, (Nat32) chan, 1, 10, ' ', UNSIGNED);
  sv203WriteData(sv, cmd);
  
  bytes = sv203ReadData(sv, rcv, 10, timeout);
  
  if (bytes == ERROR)
     return(ERROR);

  value = atoi(rcv + 1);

  return (value);
} /* 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, 0));
} /* sv203MoveAbsolute() */

/****************************************************************************/
/* Function    : sv203SetPWM                                                */
/* Purpose     : Request torque                                             */
/* Inputs      : sv, torque                                                 */
/* Outputs     : None                                                       */
/****************************************************************************/
/*
** tcounts = 122 corresponds to torque = 0;
** tcounts = 0   corresponds to torque = Positive Maximum;
** tcounts = 255 corresponds to torque = Negative Maximum;
*/
    Int16
sv203SetPWM(sv203Struct *sv, Byte pwm)
{
  char cmd[10] = "M";
  Byte delta;

  /* sv->lastMoveAbs = tcounts; */

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

  /* return (sv203ReadData(sv, cmd, 10, 0));*/
  return(OK);
} /* sv203SetPwm() */