c_output.c

Go to the documentation of this file.

/*
 * LEGO® MINDSTORMS EV3
 *
 * Copyright (C) 2010-2013 The LEGO Group
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 */


/*
 * SYNCRONIZATION:
 *
 *  Speed -100 to +100 is move forward or move backwards
 *  Turn ratio is how tight you turn and to what direction you turn
 *   - 0 value is moving straight forward
 *   - Negative values turns to the left
 *   - Positive values turns to the right
 *   - Value -100 stops the left motor
 *   - Value +100 stops the right motor
 *   - Values less than -100 makes the left motor run the opposite
 *     direction of the right motor (Spin)
 *   - Values greater than +100 makes the right motor run the opposite
 *     direction of the left motor (Spin)
 *
 *  Example: opOUTPUT_TIME_SYNC(0, 10, 100, 50, 10000,1)
 *
 *  0     = Layer
 *  10    = Motor bit field - Motor B and D
 *  100   = Motor speed     - Motor B will run at speed 100 (because ratio is positive)
 *  10    = Turn ratio      - Motor D will run at speed 50
 *  10000 = time in mS      - Motors will run for 10 sec.
 *  1     = Brake bit       - When 10 sec. has elapsed then brake both motors
 *
 *
 *  Example: opOUTPUT_TIME_SYNC(0, 10, 100, 150, 10000,1)
 *
 *  0     = Layer
 *  10    = Motor bit field - Motor B and D
 *  100   = Motor speed     - Motor B will run at speed 100 (because ratio is positive)
 *  10    = Turn ratio      - Motor D will run at speed -50
 *  10000 = time in mS      - Motors will run for 10 sec.
 *  1     = Brake bit       - When 10 sec. has elapsed then brake both motors
 *
 *
 *  Example: opOUTPUT_TIME_SYNC(0, 10, 100, -50, 10000,1)
 *
 *  0     = Layer
 *  10    = Motor bit field - Motor B and D
 *  100   = Motor speed     - Motor B will run at speed  50 (because ratio is positive)
 *  10    = Turn ratio      - Motor D will run at speed 100
 *  10000 = time in mS      - Motors will run for 10 sec.
 *  1     = Brake bit       - When 10 sec. has elapsed then brake both motors
 *
 *
 *  Example: opOUTPUT_TIME_SYNC(0, 10, 200, -150, 10000,1)
 *
 *  0     = Layer
 *  10    = Motor bit field - Motor B and D
 *  100   = Motor speed     - Motor B will run at speed -50 (because ratio is positive)
 *  10    = Turn ratio      - Motor D will run at speed 100
 *  10000 = time in mS      - Motors will run for 10 sec.
 *  1     = Brake bit       - When 10 sec. has elapsed then brake both motors
 *
 *\

\endverbatim
 *
 */


#include  "lms2012.h"
#include  "c_output.h"
#ifndef    DISABLE_DAISYCHAIN_COM_CALL
  #include  "c_daisy.h"
#endif

#if (HARDWARE != SIMULATION)

  #include  <stdio.h>
  #include  <fcntl.h>
  #include  <stdlib.h>
  #include  <unistd.h>
  #include  <string.h>
  #include  <signal.h>
  #include  <sys/mman.h>

  OUTPUT_GLOBALS OutputInstance;
  #ifndef    DISABLE_DAISYCHAIN_COM_CALL
    static  DATA8  DaisyBuf[64];
  #endif


#else

  OUTPUT_GLOBALS * gOutputInstance;

  void setOutputInstance(OUTPUT_GLOBALS * _Instance)
  {
    gOutputInstance= _Instance;
  }

  OUTPUT_GLOBALS* getOutputInstance()
  {
    return gOutputInstance;
  }

#endif

#ifdef Linux_X86
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#endif

uint DELAY_COUNTER = 0;
UBYTE   BusyOnes = 0;

void      OutputReset(void)
{
  UBYTE   Tmp;
  DATA8   StopArr[3];

  for(Tmp = 0; Tmp < OUTPUTS; Tmp++)
  {
    OutputInstance.Owner[Tmp] = 0;
  }

  StopArr[0] = (DATA8)opOUTPUT_STOP;
  StopArr[1] = 0x0F;
  StopArr[2] = 0x00;
  if (OutputInstance.PwmFile >= 0)
  {
    write(OutputInstance.PwmFile,StopArr,3);
  }
}


RESULT    cOutputInit(void)
{
  RESULT      Result = FAIL;
  MOTORDATA   *pTmp;

  // To ensure that pMotor is never uninitialised
  OutputInstance.pMotor  =  OutputInstance.MotorData;

  // Open the handle for writing commands
  OutputInstance.PwmFile  =  open(PWM_DEVICE_NAME,O_RDWR);

  if (OutputInstance.PwmFile >= 0)
  {

    // Open the handle for reading motor values - shared memory
    OutputInstance.MotorFile  =  open(MOTOR_DEVICE_NAME,O_RDWR | O_SYNC);
    if (OutputInstance.MotorFile >= 0)
    {
      pTmp  =  (MOTORDATA*)mmap(0, sizeof(OutputInstance.MotorData), PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, OutputInstance.MotorFile, 0);

      if (pTmp == MAP_FAILED)
      {
        LogErrorNumber(OUTPUT_SHARED_MEMORY);
        close(OutputInstance.MotorFile);
        close(OutputInstance.PwmFile);
      }
      else
      {
        OutputInstance.pMotor  =  (MOTORDATA*)pTmp;
        Result  =  cOutputOpen();
      }
    }
  }

  return (Result);
}


RESULT    cOutputOpen(void)
{
  RESULT  Result = FAIL;

  UBYTE   PrgStart =  opPROGRAM_START;

  OutputReset();

  if (OutputInstance.PwmFile >= 0)
  {
    write(OutputInstance.PwmFile,&PrgStart,1);
  }

  Result  =  OK;

  return (Result);
}


RESULT    cOutputClose(void)
{
  return (OK);
}


RESULT    cOutputExit(void)
{
  RESULT  Result = FAIL;

  OutputReset();

  if (OutputInstance.MotorFile >= 0)
  {
    munmap(OutputInstance.pMotor,sizeof(OutputInstance.MotorData));
    close(OutputInstance.MotorFile);
  }

  if (OutputInstance.PwmFile >= 0)
  {
    close(OutputInstance.PwmFile);
  }

  Result  =  OK;

  return (Result);
}


void    cOutputSetTypes(char *pTypes)
{
  DATA8   TypeArr[5];

  TypeArr[0] = opOUTPUT_SET_TYPE;
  memcpy(&TypeArr[1], pTypes, 4);

  if (OutputInstance.PwmFile >= 0)
  {
    write(OutputInstance.PwmFile,TypeArr,sizeof(TypeArr));
  }

}


UBYTE      cOutputPackParam(DATA32 Val, DATA8 *pStr)
{
  DATA8   Len;

  Len = 0;
  if ((Val < 32) && (Val > -32))
  {
    pStr[Len] = (DATA8)(Val & 0x0000003F);
    Len++;
  }
  else
  {
    if ((Val < DATA8_MAX) && (Val > DATA8_MIN))
    {
      pStr[Len] = 0x81;
      Len++;
      pStr[Len] = (DATA8)Val;
      Len++;
    }
    else
    {
      if ((Val < DATA16_MAX) && (Val > DATA16_MIN))
      {
        pStr[Len] = 0x82;
        Len++;
        ((UBYTE*)pStr)[Len] = (UBYTE)(Val & 0x00FF);
        Len++;
        ((UBYTE*)pStr)[Len] = (UBYTE)((Val >> 8) & 0x00FF);
        Len++;
      }
      else
      {
        pStr[Len] = 0x83;
        Len++;
        ((UBYTE*)pStr)[Len] = (UBYTE)(Val & 0x000000FF);
        Len++;
        ((UBYTE*)pStr)[Len] = (UBYTE)((Val >>  8) & 0x000000FF);
        Len++;
        ((UBYTE*)pStr)[Len] = (UBYTE)((Val >> 16) & 0x000000FF);
        Len++;
        ((UBYTE*)pStr)[Len] = (UBYTE)((Val >> 24) & 0x000000FF);
        Len++;
      }
    }
  }
  return(Len);
}
/*

UBYTE     cMotorGetBusyFlags(void)
{
  int     test, test2;
  char    BusyReturn[10]; // Busy mask

  if (OutputInstance.PwmFile >= 0)
  {
    read(OutputInstance.PwmFile,BusyReturn,4);
    sscanf(BusyReturn,"%u %u",&test,&test2);
  }
  else
  {
    test = 0;
  }
  printf("cMotorGetBusyFlags test = %d\n\r", test);
  return(test);
}*/

void ResetDelayCounter(UBYTE Pattern)
{
    BusyOnes = Pattern;
    DELAY_COUNTER = 0;
}

UBYTE     cMotorGetBusyFlags(void)
{
  int     test, test2;
  char    BusyReturn[10]; // Busy mask

  if (OutputInstance.PwmFile >= 0)
  {
    read(OutputInstance.PwmFile,BusyReturn,4);
    sscanf(BusyReturn,"%u %u",&test,&test2);
  }
  else
  {
    test = 0;
  }
  if(DELAY_COUNTER < 25)
  {
      test = BusyOnes;
      DELAY_COUNTER++;
  }

  return(test);
}


void      cMotorSetBusyFlags(UBYTE Flags)
{
  if (OutputInstance.MotorFile >= 0)
  {
    write(OutputInstance.MotorFile, &Flags, sizeof(Flags));
  }
}



//******* BYTE CODE SNIPPETS **************************************************


void      cOutputPrgStop(void)
{
  DSPSTAT DspStat = NOBREAK;
  DATA8   PrgStop;

  PrgStop = (DATA8)opPROGRAM_STOP;
  if (OutputInstance.PwmFile >= 0)
  {
    write(OutputInstance.PwmFile,&PrgStop,1);
  }
  SetDispatchStatus(DspStat);
}


void      cOutputSetType(void)
{
  DATA8   Layer;
  DATA8   No;
  DATA8   Type;
  UBYTE   Len;
  DSPSTAT DspStat = NOBREAK;
  IP      TmpIp;

  TmpIp  =  GetObjectIp();
  Len    =  0;
  Layer  =  *(DATA8*)PrimParPointer();
  No     =  *(DATA8*)PrimParPointer();
  Type   =  *(DATA8*)PrimParPointer();

  if (Layer == 0)
  {
    if ((No >= 0) && (No < OUTPUTS))
    {
      if (OutputInstance.OutputType[No] != Type)
      {
        OutputInstance.OutputType[No]  =  Type;

        if ((Type == TYPE_NONE) || (Type == TYPE_ERROR))
        {
#ifdef DEBUG
          printf("                Output %c Disable\r\n",'A' + No);
#endif
        }
        else
        {
#ifdef DEBUG
          printf("                Output %c Enable\r\n",'A' + No);
#endif
        }
      }
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_RESET;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)No, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Type, &(DaisyBuf[Len]));
        if(OK != cDaisyDownStreamCmd(DaisyBuf, Len, Layer))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        //cDaisyDownStreamCmd(DaisyBuf, Len, Layer);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputReset(void)
{
  DATA8   Layer;
  UBYTE   Nos;
  UBYTE   Len;
  DSPSTAT DspStat = NOBREAK;
  IP      TmpIp;

  DATA8   ResetArr[2];

  TmpIp  =  GetObjectIp();
  Len    =  0;
  Layer  =  *(DATA8*)PrimParPointer();
  Nos    =  *(DATA8*)PrimParPointer();

  if (Layer == 0)
  {

    ResetArr[0] = opOUTPUT_RESET;
    ResetArr[1] = Nos;

    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,ResetArr,sizeof(ResetArr));
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_RESET;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Nos, &(DaisyBuf[Len]));
        if(OK != cDaisyDownStreamCmd(DaisyBuf, Len, Layer))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        //cDaisyDownStreamCmd(DaisyBuf, Len, Layer);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputStop(void)
{
  DATA8   Layer;
  UBYTE   Nos;
  UBYTE   Brake;
  UBYTE   Len;
  DSPSTAT DspStat = NOBREAK;
  IP      TmpIp;

  DATA8   StopArr[3];

  TmpIp  =  GetObjectIp();
  Len    =  0;
  Layer  =  *(DATA8*)PrimParPointer();
  Nos    =  *(DATA8*)PrimParPointer();
  Brake  =  *(DATA8*)PrimParPointer();

  if (Layer == 0)
  {
    StopArr[0] = (DATA8)opOUTPUT_STOP;
    StopArr[1] = Nos;
    StopArr[2] = Brake;

    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,StopArr,sizeof(StopArr));
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_STOP;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Nos, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Brake, &(DaisyBuf[Len]));
        if(OK != cDaisyDownStreamCmd(DaisyBuf, Len, Layer))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        //cDaisyDownStreamCmd(DaisyBuf, Len, Layer);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputSpeed(void)
{
  DATA8   Layer;
  UBYTE   Nos;
  DATA8   Speed;
  UBYTE   Len;
  DSPSTAT DspStat = NOBREAK;
  IP      TmpIp;

  DATA8   SetSpeed[3];

  TmpIp  =  GetObjectIp();
  Len    =  0;
  Layer  =  *(DATA8*)PrimParPointer();
  Nos    =  *(DATA8*)PrimParPointer();
  Speed  =  *(DATA8*)PrimParPointer();

  if (Layer == 0)
  {


    SetSpeed[0] = (DATA8)opOUTPUT_SPEED;
    SetSpeed[1] = Nos;
    SetSpeed[2] = Speed;

    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,SetSpeed,sizeof(SetSpeed));
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_SPEED;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Nos, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Speed, &(DaisyBuf[Len]));
        if(OK != cDaisyDownStreamCmd(DaisyBuf, Len, Layer))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputPower(void)
{
  DATA8   Layer;
  UBYTE   Nos;
  DATA8   Power;
  DATA8   SetPower[3];
  DATA8   Len;
  DSPSTAT DspStat = NOBREAK;
  IP      TmpIp;

  TmpIp  =  GetObjectIp();
  Len    =  0;
  Layer  =  *(DATA8*)PrimParPointer();
  Nos    =  *(DATA8*)PrimParPointer();
  Power  =  *(DATA8*)PrimParPointer();

  if (Layer == 0)
  {
    SetPower[0] = (DATA8)opOUTPUT_POWER;
    SetPower[1] = Nos;
    SetPower[2] = Power;
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,SetPower,sizeof(SetPower));
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_POWER;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Nos, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Power, &(DaisyBuf[Len]));

        if(OK != cDaisyDownStreamCmd(DaisyBuf, Len, Layer))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        else
        {
          // printf("cOutPut @ opOUTPUT_POWER after cDaisyDownStreamCmd - OK and WriteState = %d\n\r", cDaisyGetLastWriteState());
        }
        //cDaisyDownStreamCmd(DaisyBuf, Len, Layer);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputStart(void)
{
  DATA8   Tmp;
  DATA8   Layer;
  UBYTE   Nos;
  DATA8   Len;
  DSPSTAT DspStat = NOBREAK;
  IP      TmpIp;

  DATA8   StartMotor[2];

  TmpIp  =  GetObjectIp();
  Len    =  0;
  Layer  =  *(DATA8*)PrimParPointer();
  Nos    =  *(DATA8*)PrimParPointer();

  if (Layer == 0)
  {
    StartMotor[0] = (DATA8)opOUTPUT_START;
    StartMotor[1] = Nos;
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,StartMotor,sizeof(StartMotor));

      for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
      {
        if (Nos & (0x01 << Tmp))
        {
          OutputInstance.Owner[Tmp] = CallingObjectId();
        }
      }
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_START;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Nos, &(DaisyBuf[Len]));
        if(OK != cDaisyDownStreamCmd(DaisyBuf, Len, Layer))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        else
        {
          //printf("cOutPut @ opOUTPUT_START after cDaisyDownStreamCmd - OK and WriteState = %d\n\r", cDaisyGetLastWriteState());
        }
        //cDaisyDownStreamCmd(DaisyBuf, Len, Layer);

      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputPolarity(void)
{
  DATA8   Layer;
  DATA8   Polarity[3];
  UBYTE   Len;
  DSPSTAT DspStat = NOBREAK;
  IP      TmpIp;

  TmpIp       =  GetObjectIp();
  Len         =  0;
  Layer       =  *(DATA8*)PrimParPointer();
  Polarity[0] =   (DATA8)opOUTPUT_POLARITY;
  Polarity[1] =  *(DATA8*)PrimParPointer();
  Polarity[2] =  *(DATA8*)PrimParPointer();

  if (Layer == 0)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,Polarity,sizeof(Polarity));
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_POLARITY;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Polarity[1], &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)Polarity[2], &(DaisyBuf[Len]));
        if(OK != cDaisyDownStreamCmd(DaisyBuf, Len, Layer))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        //cDaisyDownStreamCmd(DaisyBuf, Len, Layer);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputStepPower(void)
{
  DATA8       Layer;
  DATA8       Tmp;
  STEPPOWER   StepPower;
  UBYTE       Len;
  DSPSTAT     DspStat = NOBREAK;
  IP          TmpIp;

  TmpIp            =  GetObjectIp();
  Len              =  0;
  Layer            =  *(DATA8*)PrimParPointer();
  StepPower.Cmd    =   opOUTPUT_STEP_POWER;
  StepPower.Nos    =  *(DATA8*)PrimParPointer();
  StepPower.Power  =  *(DATA8*)PrimParPointer();
  StepPower.Step1  =  *(DATA32*)PrimParPointer();
  StepPower.Step2  =  *(DATA32*)PrimParPointer();
  StepPower.Step3  =  *(DATA32*)PrimParPointer();
  StepPower.Brake  =  *(DATA8*)PrimParPointer();

  if (0 == Layer)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,(DATA8*)&(StepPower.Cmd),sizeof(StepPower));

      for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
      {
        // Set calling id for all involved inputs
        if (StepPower.Nos & (0x01 << Tmp))
        {
          OutputInstance.Owner[Tmp] = CallingObjectId();
        }
      }
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_STEP_POWER;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepPower.Nos,   &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepPower.Power, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepPower.Step1, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepPower.Step2, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepPower.Step3, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepPower.Brake, &(DaisyBuf[Len]));

        //if(OK != cDaisyDownStreamCmd(DaisyBuf, Len, Layer))
        if(OK != cDaisyMotorDownStream(DaisyBuf, Len, Layer, StepPower.Nos))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }

        //cDaisyMotorDownStream(DaisyBuf, Len, Layer, StepPower.Nos);

      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputTimePower(void)
{
  DATA8       Layer;
  DATA8       Tmp;
  TIMEPOWER   TimePower;
  UBYTE       Len;
  DSPSTAT     DspStat = NOBREAK;
  IP          TmpIp;

  TmpIp            =  GetObjectIp();
  Len              =  0;
  Layer            =  *(DATA8*) PrimParPointer();
  TimePower.Cmd    =  opOUTPUT_TIME_POWER;
  TimePower.Nos    =  *(DATA8*) PrimParPointer();
  TimePower.Power  =  *(DATA8*) PrimParPointer();
  TimePower.Time1  =  *(DATA32*)PrimParPointer();
  TimePower.Time2  =  *(DATA32*)PrimParPointer();
  TimePower.Time3  =  *(DATA32*)PrimParPointer();
  TimePower.Brake  =  *(DATA8*) PrimParPointer();

  if (0 == Layer)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,(DATA8*)&(TimePower.Cmd),sizeof(TimePower));

      for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
      {
        // Set calling id for all involved inputs
        if (TimePower.Nos & (0x01 << Tmp))
        {
          OutputInstance.Owner[Tmp] = CallingObjectId();
        }
      }
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_TIME_POWER;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimePower.Nos,   &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimePower.Power, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimePower.Time1, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimePower.Time2, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimePower.Time3, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimePower.Brake, &(DaisyBuf[Len]));
        if(OK != cDaisyMotorDownStream(DaisyBuf, Len, Layer, TimePower.Nos))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        //cDaisyMotorDownStream(DaisyBuf, Len, Layer, TimePower.Nos);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}

void      cOutputStepSpeed(void)
{
  DATA8       Layer;
  DATA8       Tmp;
  STEPSPEED   StepSpeed;
  UBYTE       Len;
  DSPSTAT     DspStat = NOBREAK;
  IP          TmpIp;

  //DEBUG
//  int i;

  TmpIp            =  GetObjectIp();
  Len              =  0;
  Layer            =  *(DATA8*)PrimParPointer();
  StepSpeed.Cmd    =   opOUTPUT_STEP_SPEED;
  StepSpeed.Nos    =  *(DATA8*)PrimParPointer();
  StepSpeed.Speed  =  *(DATA8*)PrimParPointer();
  StepSpeed.Step1  =  *(DATA32*)PrimParPointer();
  StepSpeed.Step2  =  *(DATA32*)PrimParPointer();
  StepSpeed.Step3  =  *(DATA32*)PrimParPointer();
  StepSpeed.Brake  =  *(DATA8*)PrimParPointer();
/*
  printf("StepSpeed.Cmd = %d\n\r", StepSpeed.Cmd);
  printf("StepSpeed.Nos = %d\n\r", StepSpeed.Nos);
  printf("StepSpeed.Speed = %d\n\r", StepSpeed.Speed);
  printf("StepSpeed.Step1 = %d\n\r", StepSpeed.Step1);
  printf("StepSpeed.Step2 = %d\n\r", StepSpeed.Step2);
  printf("StepSpeed.Step3 = %d\n\r", StepSpeed.Step3);
  printf("StepSpeed.Brake = %d\n\r", StepSpeed.Brake);

*/
  if (0 == Layer)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,(DATA8*)&(StepSpeed.Cmd),sizeof(StepSpeed));

      for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
      {// Set calling id for all involved inputs

        if (StepSpeed.Nos & (0x01 << Tmp))
        {
          OutputInstance.Owner[Tmp] = CallingObjectId();
        }
      }
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_STEP_SPEED;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSpeed.Nos,   &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSpeed.Speed, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSpeed.Step1, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSpeed.Step2, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSpeed.Step3, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSpeed.Brake, &(DaisyBuf[Len]));

       /* printf("Len = %d\n\r", Len);
        for(i = 0; i < Len; i++)
                            printf("DaisyBuf[%d]= %x\n\r", i, DaisyBuf[i]);
                          printf("\n\r");
*/

        if(OK != cDaisyMotorDownStream(DaisyBuf, Len, Layer, StepSpeed.Nos))
        {
          printf("NOT ok txed cOutputStepSpeed\n\r");
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        else
        {
  /*        for(i = 0; i < Len; i++)
                    printf("DaisyBuf[%d]= %d, ", i, DaisyBuf[i]);
                  printf("\n\r");
*/
        }
        //cDaisyMotorDownStream(DaisyBuf, Len, Layer, StepSpeed.Nos);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputTimeSpeed(void)
{
  DATA8       Layer;
  DATA8       Tmp;
  TIMESPEED   TimeSpeed;
  UBYTE       Len;
  DSPSTAT     DspStat = NOBREAK;
  IP          TmpIp;

  TmpIp            =  GetObjectIp();
  Len              =  0;
  Layer            =  *(DATA8*)PrimParPointer();
  TimeSpeed.Cmd    =   (DATA8)opOUTPUT_TIME_SPEED;
  TimeSpeed.Nos    =  *(DATA8*)PrimParPointer();
  TimeSpeed.Speed  =  *(DATA8*)PrimParPointer();
  TimeSpeed.Time1  =  *(DATA32*)PrimParPointer();
  TimeSpeed.Time2  =  *(DATA32*)PrimParPointer();
  TimeSpeed.Time3  =  *(DATA32*)PrimParPointer();
  TimeSpeed.Brake  =  *(DATA8*)PrimParPointer();

  if (0 == Layer)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,(DATA8*)&(TimeSpeed.Cmd),sizeof(TimeSpeed));

      for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
      {
        // Set calling id for all involved inputs
        if (TimeSpeed.Nos & (0x01 << Tmp))
        {
          OutputInstance.Owner[Tmp] = CallingObjectId();
        }
      }
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_TIME_SPEED;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSpeed.Nos,   &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSpeed.Speed, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSpeed.Time1, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSpeed.Time2, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSpeed.Time3, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSpeed.Brake, &(DaisyBuf[Len]));
        if(OK != cDaisyMotorDownStream(DaisyBuf, Len, Layer, TimeSpeed.Nos))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        //cDaisyMotorDownStream(DaisyBuf, Len, Layer, TimeSpeed.Nos);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputStepSync(void)
{
  DATA8       Layer;
  DATA8       Tmp;
  STEPSYNC    StepSync;
  UBYTE       Len;
  DSPSTAT     DspStat = NOBREAK;
  IP          TmpIp;

  TmpIp          =  GetObjectIp();
  Len            =  0;
  Layer          =  *(DATA8*)PrimParPointer();
  StepSync.Cmd   =   opOUTPUT_STEP_SYNC;
  StepSync.Nos   =  *(DATA8*)PrimParPointer();
  StepSync.Speed =  *(DATA8*)PrimParPointer();
  StepSync.Turn  =  *(DATA16*)PrimParPointer();
  StepSync.Step  =  *(DATA32*)PrimParPointer();
  StepSync.Brake =  *(DATA8*)PrimParPointer();

  if (0 == Layer)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,(DATA8*)&(StepSync.Cmd),sizeof(StepSync));

      for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
      {
        // Set calling id for all involved outputs
        if (StepSync.Nos & (0x01 << Tmp))
        {
          OutputInstance.Owner[Tmp] = CallingObjectId();
        }
      }
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_STEP_SYNC;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSync.Nos,   &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSync.Speed, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSync.Turn,  &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSync.Step,  &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)StepSync.Brake, &(DaisyBuf[Len]));
        if(OK != cDaisyMotorDownStream(DaisyBuf, Len, Layer, StepSync.Nos))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        //cDaisyMotorDownStream(DaisyBuf, Len, Layer, StepSync.Nos);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputTimeSync(void)
{
  DATA8     Layer;
  DATA8     Tmp;
  TIMESYNC  TimeSync;
  UBYTE     Len;
  DSPSTAT   DspStat = NOBREAK;
  IP        TmpIp;

  TmpIp          =  GetObjectIp();
  Len            =  0;
  Layer          =  *(DATA8*)PrimParPointer();
  TimeSync.Cmd   =  opOUTPUT_TIME_SYNC;
  TimeSync.Nos   =  *(DATA8*)PrimParPointer();
  TimeSync.Speed =  *(DATA8*)PrimParPointer();
  TimeSync.Turn  =  *(DATA16*)PrimParPointer();
  TimeSync.Time  =  *(DATA32*)PrimParPointer();
  TimeSync.Brake =  *(DATA8*)PrimParPointer();

  if (0 == Layer)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile,(DATA8*)&(TimeSync.Cmd),sizeof(TimeSync));

      for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
      {
        // Set calling id for all involved outputs
        if (TimeSync.Nos & (0x01 << Tmp))
        {
          OutputInstance.Owner[Tmp] = CallingObjectId();
        }
      }
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_TIME_SYNC;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSync.Nos,   &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSync.Speed, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSync.Turn,  &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSync.Time,  &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)TimeSync.Brake, &(DaisyBuf[Len]));
        if(OK != cDaisyMotorDownStream(DaisyBuf, Len, Layer, TimeSync.Nos))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        //cDaisyMotorDownStream(DaisyBuf, Len, Layer, TimeSync.Nos);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputRead(void)
{

  DATA8   Layer;
  DATA8   No;
  DATA8   Speed = 0;
  DATA32  Tacho = 0;

  Layer  = *(DATA8*)PrimParPointer();
  No     = *(DATA8*)PrimParPointer();

  if (0 == Layer)
  {
    if (No < OUTPUTS)
    {
      Speed  =  OutputInstance.pMotor[No].Speed;
      Tacho  =  OutputInstance.pMotor[No].TachoCounts;
    }
  }
  *(DATA8*)PrimParPointer()   =  Speed;
  *(DATA32*)PrimParPointer()  =  Tacho;
}



void      cOutputReady(void)
{

  OBJID   Owner;
  DATA8   Layer, Tmp, Nos;
  IP      TmpIp;
  DSPSTAT DspStat = NOBREAK;
  UBYTE   Bits;

  int     test;
  int     test2;

  char    BusyReturn[10]; // Busy mask

  TmpIp  =  GetObjectIp();

  Layer  = *(DATA8*)PrimParPointer();
  Nos    = *(DATA8*)PrimParPointer();
  Owner  =  CallingObjectId();

  if (0 == Layer)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      read(OutputInstance.PwmFile,BusyReturn,4);

      sscanf(BusyReturn,"%u %u",&test,&test2);

      for (Tmp = 0;(Tmp < OUTPUTS) && (DspStat == NOBREAK);Tmp++)
      {
        // Check resources for NOTREADY
        if (Nos & (1 << Tmp))
        {
          // Only relevant ones
          if (test & (1 << Tmp))
          {
            // If BUSY check for OVERRULED
            if (OutputInstance.Owner[Tmp] == Owner)
            {
              DspStat  =  BUSYBREAK;
            }
          }
        }
      }
    }
  }
  else
  {
    Bits = cDaisyCheckBusyBit(Layer, Nos);
    Bits = 0;

    for (Tmp = 0;(Tmp < OUTPUTS) && (DspStat == NOBREAK);Tmp++)
    {
      // Check resources for NOTREADY
      if (Nos & (1 << Tmp))
      {
        // Only relevant ones
        if (Bits & (1 << Tmp))
        {
          // If BUSY check for OVERRULED
          if (OutputInstance.Owner[Tmp] == Owner)
          {
            DspStat  =  BUSYBREAK;
          }
        }
      }
    }
  }

  if (DspStat == BUSYBREAK)
  {
    // Rewind IP
    SetObjectIp(TmpIp - 1);
  }
  SetDispatchStatus(DspStat);
}


void      cOutputTest(void)
{

  DATA8   Layer, Nos, Busy = 0;

  int     test;
  int     test2;

  char    BusyReturn[20]; // Busy mask

  Layer  = *(DATA8*)PrimParPointer();
  Nos    = *(DATA8*)PrimParPointer();

  if (0 == Layer)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      read(OutputInstance.PwmFile,BusyReturn,10);
      sscanf(BusyReturn,"%u %u",&test,&test2);

      if (Nos & (DATA8)test2)
      {
        Busy = 1;
      }
    }
  }
  else
  {
    if (cDaisyCheckBusyBit(Layer, Nos))
    {
      Busy = 1;
    }
  }
  *(DATA8*)PrimParPointer() = Busy;
}


void      cOutputClrCount(void)
{
  DATA8   Layer;
  DATA8   ClrCnt[2];
  UBYTE   Len;
  DSPSTAT DspStat = NOBREAK;
  IP      TmpIp;
  UBYTE   Tmp;

  TmpIp       =  GetObjectIp();
  Len         =  0;
  Layer       =  *(DATA8*)PrimParPointer();
  ClrCnt[0]   =  opOUTPUT_CLR_COUNT;
  ClrCnt[1]   =  *(DATA8*)PrimParPointer();

  if (0 == Layer)
  {
    if (OutputInstance.PwmFile >= 0)
    {
      write(OutputInstance.PwmFile, ClrCnt, sizeof(ClrCnt));
    }

    // Also the user layer entry to get immediate clear
    for(Tmp = 0; Tmp < OUTPUTS; Tmp++)
    {
      if (ClrCnt[1] & (1 << Tmp))
      {
        OutputInstance.pMotor[Tmp].TachoSensor = 0;
      }
    }
  }
  else
  {
    #ifndef    DISABLE_DAISYCHAIN_COM_CALL
      if (cDaisyReady() != BUSY)
      {
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  0;
        DaisyBuf[Len++]  =  opOUTPUT_CLR_COUNT;
        Len             +=  cOutputPackParam((DATA32)0, &(DaisyBuf[Len]));
        Len             +=  cOutputPackParam((DATA32)ClrCnt[1], &(DaisyBuf[Len]));
        if(OK != cDaisyDownStreamCmd(DaisyBuf, Len, Layer))
        {
          SetObjectIp(TmpIp - 1);
          DspStat  =  BUSYBREAK;
        }
        //cDaisyDownStreamCmd(DaisyBuf, Len, Layer);
      }
      else
      {
        SetObjectIp(TmpIp - 1);
        DspStat  =  BUSYBREAK;
      }
    #endif
  }
  SetDispatchStatus(DspStat);
}


void      cOutputGetCount(void)
{
  DATA8   Layer;
  DATA8   No;
  DATA32  Tacho;

  Layer  = *(DATA8*)PrimParPointer();
  No     = *(DATA8*)PrimParPointer();

  if (0 == Layer)
  {
    if (No < OUTPUTS)
    {
      Tacho  =  OutputInstance.pMotor[No].TachoSensor;
    }
  }
  *(DATA32*)PrimParPointer()  =  Tacho;
}


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