/****************************************************************************/
/* Copyright (c) 2000 MBARI.                                                */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : STP100 Stepper Motor Controller - Function Definitions        */
/* Filename : stp100.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 "stp100.h"

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

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

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

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

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

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

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

    Int16
stp100WriteData(stpStruct *stp, char *cmd)
{
  char term[2];

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

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

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

  term[0] = STP100_TERM;
  term[1] = 0;

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


    Int16
stp100ReadData(stpStruct *stp, Byte *buffer, Int16 maxLen)
{
    Int16  status;

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

    while (serialBoardTermRecvd(stp->serialChan.boardEntry,
            stp->serialChan.channel) == 0);

    status = serialBoardRead(stp->serialChan.boardEntry,
            stp->serialChan.channel, buffer, maxLen);

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


    Int16
stp100CmdString(char *cmd, const char *cmdStr, Int16 value)
{
  char *v, val[10];

  itoa(val, (Nat32) value, 8, 10, ' ', DOSIGNED);
  v = val;
  while (isspace(*v))
      (v++);

  strcpy(cmd, cmdStr);
  strcpy(cmd + strlen(cmd), v);
} /* stp100CmdString */

    Int16
stp100BoardSelect(stpStruct *stp, Int16 board)
{
  char cmd[10];

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

  stp100CmdString(cmd, "BD", board);
  stp100WriteData(stp, cmd);
} /* stp100BoardSelect() */

    Int16
stp100ReadID(stpStruct *stp)
{
  char cmd[10];

  stp100WriteData(stp, "");

  return (stp100ReadData(stp, cmd, 10));
} /* stp100ReadID() */


    Int16
stp100ReadAtoD(stpStruct *stp, Int16 chan)
{
  Int16 atod;
  char cmd[10];

  stp100CmdString(cmd, "AD", chan);
  stp100WriteData(stp, cmd);

  stp100ReadData(stp, cmd, 10);

  atod = atoi(cmd + 1);
  return (atod);
} /* stp100ReadAtoD() */


    Int16
stp100StepDelay(stpStruct *stp, Nat16 delay)
{
  char cmd[10];

  if (delay < 6)
    delay = 6;                  /* Minimum value is 6 */

  stp100CmdString(cmd, "SD", delay);
  stp100WriteData(stp, cmd);
} /* stp100StepDelay() */


    Int16
stp100VelocityProfile(stpStruct *stp, Nat16 minStepDelay, Nat16 accelFactor)
{
  char cmd[10];

  stp100CmdString(cmd, "SM", minStepDelay);
  stp100WriteData(stp, cmd);

  stp100CmdString(cmd, "SA", accelFactor);
  stp100WriteData(stp, cmd);

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


    Int16
stp100HaltMovement(stpStruct *stp)
{
  stp100WriteData(stp, "HI");
} /* stp100SetHaltMovement() */


    Int16
stp100MoveClockwise(stpStruct *stp)
{
  stp100WriteData(stp, "H+");
} /* stp100SetMoveClockwise() */

    Int16
stp100MoveCounterCW(stpStruct *stp)
{
  stp100WriteData(stp, "H-");
} /* stp100SetMoveCounterCW() */


    Int16
stp100MoveAbsolute(stpStruct *stp, Int16 position)
{
  char cmd[20];

  stp100CmdString(cmd, "MI", position);
  stp100WriteData(stp, cmd);
} /* stp100SetMoveAbsolute() */

    Int16
stp100Position(stpStruct *stp)
{
  Int16 position;
                                    /* 1 specifies the position feedback */
  position = stp100ReadAtoD(stp, 1);

  return(position - 128);
} /* stp100Position() */


    Int16
stp100HighGainPosition(stpStruct *stp)
{
  Int16 position;
                                    /* 2 specifies the position feedback */
  position = stp100ReadAtoD(stp, 2);

  return(position);
} /* stp100HighGainPosition() */


    Int16
stp100SetHomePosition(stpStruct *stp)
{
  stp100WriteData(stp, "HM0");
} /* stp100SetHomePosition() */


   Int16
stp100FindHome(stpStruct *stp)
{
    Int16 delta;
    Int16 position;

    stp100MoveClockwise(stp);

    do
    {
       position = stp100HighGainPosition(stp);
       delta = position - 128;

       if (abs(delta) > 64)
           stp100StepDelay(stp, 500);
        else
           stp100StepDelay(stp, 20000);

        if (delta <= 0)
          stp100MoveClockwise(stp);
        else
          stp100MoveCounterCW(stp);
    } while (abs(delta) > 1);

    stp100HaltMovement(stp);

    stp100SetHomePosition(stp);

    stp100StepDelay(stp, 512);
} /* stp100FindHome() */


    Void
stp100InitializeController(stpStruct *stp, Nat16 controlPeriod)
{
                                    /* Start controller timebase             */
  stp->controlTick = read_sysclock();
  stp->controlPeriod = controlPeriod;

  stp->desiredPosition = 0;         /* Desired position. Input to ctrl       */
  stp->position        = 0;         /* Measured position feedback            */
  stp->Kp              = 10;        /* Position Control Gain                 */
  stp->lastPosition    = 0;         /* Last Position Command                 */
} /* stp100InitializeController() */

    Void
stp100PositionControl(stpStruct *stp)
{
  Nat16 ticks;
  Int16 delta, cmdPosition;

  ticks = read_sysclock();

  delta = (Int16) ( ((Nat32) ticks) - stp->controlTick);

  if (delta < 0)                /* account for 16 bit timer overflow        */
    delta += 32768;

  if (delta < stp->controlPeriod) /* Is it time to run the controller ?      */
    return;
                               /* Restart controller timebase                */
  stp->controlTick = ticks;

  stp->position = stp100Position(stp);

  cmdPosition = stp->Kp * (stp->desiredPosition - stp->position);

#if 0
  if (abs(cmdPosition - stp->lastPosition) < STP100_DELTA_POS)
    stp100HaltMovement(stp);
  else
    stp100MoveAbsolute(stp, cmdPosition);
#endif
  stp100MoveAbsolute(stp, cmdPosition);

  stp->lastPosition = cmdPosition;

  return;
} /* stp100PositionControl() */