c_cmd.c

乐高机器人的源码,开发平台是IAR_for_AVR.

//
// Date init       14.12.2004
//
// Revision date   $Date:: 28-09-06 9:07                                     $
//
// Filename        $Workfile:: c_cmd.c                                       $
//
// Version         $Revision:: 66                                            $
//
// Archive         $Archive:: /LMS2006/Sys01/Main/Firmware/Source/c_cmd.c    $
//
// Platform        C
//

//
// File Description:
// This file contains the virtual machine implementation to run bytecode
// programs compatible with LEGO MINDSTORMS NXT Software 1.0.
//
// This module (c_cmd) is also responsible for reading the system timer
// (d_timer) and returning on 1 ms timer boundaries.
//

#include "stdconst.h"
#include "modules.h"

#include "c_cmd.iom"
#include "c_output.iom"
#include "c_input.iom"
#include "c_loader.iom"
#include "c_ui.iom"
#include "c_sound.iom"
#include "c_button.iom"
#include "c_display.iom"
#include "c_comm.iom"
#include "c_lowspeed.iom"

#include "c_cmd.h"
#include "c_cmd_bytecodes.h"
#include "d_timer.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>


static  IOMAPCMD  IOMapCmd;
static  VARSCMD   VarsCmd;
static  HEADER    **pHeaders;

const     HEADER  cCmd =
{
  0x00010001L,
  "Command",
  cCmdInit,
  cCmdCtrl,
  cCmdExit,
  (void *)&IOMapCmd,
  (void *)&VarsCmd,
  (UWORD)sizeof(IOMapCmd),
  (UWORD)sizeof(VarsCmd),
  0x0000                      //Code size - not used so far
};

#if ENABLE_VM

// c_cmd_drawing.inc is just another C source file
// (the graphics implementation was split off for practical file management reasons)
#include "c_cmd_drawing.inc"


//
//Function pointers to sub-interpreters
//This table is indexed by arity
//Unary operations can have arity of 1 or 2 (some need a destination)
//All instructions taking 4 or more operands are handled as "Other"
//
static pInterp InterpFuncs[INTERP_COUNT] =
{
  cCmdInterpNoArg,
  cCmdInterpUnop1,
  cCmdInterpUnop2,
  cCmdInterpBinop,
  cCmdInterpOther
};

//
//Function pointers to SysCall implementations
//See interpreter for OP_SYSCALL
//
static pSysCall SysCallFuncs[SYSCALL_COUNT] =
{
  cCmdWrapFileOpenRead,
  cCmdWrapFileOpenWrite,
  cCmdWrapFileOpenAppend,
  cCmdWrapFileRead,
  cCmdWrapFileWrite,
  cCmdWrapFileClose,
  cCmdWrapFileResolveHandle,
  cCmdWrapFileRename,
  cCmdWrapFileDelete,
  cCmdWrapSoundPlayFile,
  cCmdWrapSoundPlayTone,
  cCmdWrapSoundGetState,
  cCmdWrapSoundSetState,
  cCmdWrapDrawText,
  cCmdWrapDrawPoint,
  cCmdWrapDrawLine,
  cCmdWrapDrawCircle,
  cCmdWrapDrawRect,
  cCmdWrapDrawPicture,
  cCmdWrapSetScreenMode,
  cCmdWrapReadButton,
  cCmdWrapCommLSWrite,
  cCmdWrapCommLSRead,
  cCmdWrapCommLSCheckStatus,
  cCmdWrapRandomNumber,
  cCmdWrapGetStartTick,
  cCmdWrapMessageWrite,
  cCmdWrapMessageRead,
  cCmdWrapCommBTCheckStatus,
  cCmdWrapCommBTWrite,
  cCmdWrapCommBTRead,
  cCmdWrapKeepAlive,
  cCmdWrapIOMapRead,
  cCmdWrapIOMapWrite
};

//
//Next set of arrays are lookup tables for IOM access bytecodes
//
TYPE_CODE IO_TYPES_IN[IO_IN_FIELD_COUNT] =
{
  //IO_IN0
  TC_UBYTE, //IO_IN_TYPE
  TC_UBYTE, //IO_IN_MODE
  TC_UWORD, //IO_IN_ADRAW
  TC_UWORD, //IO_IN_NORMRAW
  TC_SWORD, //IO_IN_SCALED_VAL
  TC_UBYTE, //IO_IN_INVALID_DATA

  //IO_IN1
  TC_UBYTE, //IO_IN_TYPE
  TC_UBYTE, //IO_IN_MODE
  TC_UWORD, //IO_IN_ADRAW
  TC_UWORD, //IO_IN_NORMRAW
  TC_SWORD, //IO_IN_SCALED_VAL
  TC_UBYTE, //IO_IN_INVALID_DATA

  //IO_IN2
  TC_UBYTE, //IO_IN_TYPE
  TC_UBYTE, //IO_IN_MODE
  TC_UWORD, //IO_IN_ADRAW
  TC_UWORD, //IO_IN_NORMRAW
  TC_SWORD, //IO_IN_SCALED_VAL
  TC_UBYTE, //IO_IN_INVALID_DATA

  //IO_IN3
  TC_UBYTE, //IO_IN_TYPE
  TC_UBYTE, //IO_IN_MODE
  TC_UWORD, //IO_IN_ADRAW
  TC_UWORD, //IO_IN_NORMRAW
  TC_SWORD, //IO_IN_SCALED_VAL
  TC_UBYTE, //IO_IN_INVALID_DATA
};

TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] =
{
  //IO_OUT0
  TC_UBYTE, //IO_OUT_FLAGS
  TC_UBYTE, //IO_OUT_MODE
  TC_SBYTE, //IO_OUT_SPEED
  TC_SBYTE, //IO_OUT_ACTUAL_SPEED
  TC_SLONG, //IO_OUT_TACH_COUNT
  TC_ULONG, //IO_OUT_TACH_LIMIT
  TC_UBYTE, //IO_OUT_RUN_STATE
  TC_SBYTE, //IO_OUT_TURN_RATIO
  TC_UBYTE, //IO_OUT_REG_MODE
  TC_UBYTE, //IO_OUT_OVERLOAD
  TC_UBYTE, //IO_OUT_REG_P_VAL
  TC_UBYTE, //IO_OUT_REG_I_VAL
  TC_UBYTE, //IO_OUT_REG_D_VAL
  TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
  TC_SLONG, //IO_OUT_ROTATION_COUNT

  //IO_OUT1
  TC_UBYTE, //IO_OUT_FLAGS
  TC_UBYTE, //IO_OUT_MODE
  TC_SBYTE, //IO_OUT_SPEED
  TC_SBYTE, //IO_OUT_ACTUAL_SPEED
  TC_SLONG, //IO_OUT_TACH_COUNT
  TC_ULONG, //IO_OUT_TACH_LIMIT
  TC_UBYTE, //IO_OUT_RUN_STATE
  TC_SBYTE, //IO_OUT_TURN_RATIO
  TC_UBYTE, //IO_OUT_REG_MODE
  TC_UBYTE, //IO_OUT_OVERLOAD
  TC_UBYTE, //IO_OUT_REG_P_VAL
  TC_UBYTE, //IO_OUT_REG_I_VAL
  TC_UBYTE, //IO_OUT_REG_D_VAL
  TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
  TC_SLONG, //IO_OUT_ROTATION_COUNT

  //IO_OUT2
  TC_UBYTE, //IO_OUT_FLAGS
  TC_UBYTE, //IO_OUT_MODE
  TC_SBYTE, //IO_OUT_SPEED
  TC_SBYTE, //IO_OUT_ACTUAL_SPEED
  TC_SLONG, //IO_OUT_TACH_COUNT
  TC_ULONG, //IO_OUT_TACH_LIMIT
  TC_UBYTE, //IO_OUT_RUN_STATE
  TC_SBYTE, //IO_OUT_TURN_RATIO
  TC_UBYTE, //IO_OUT_REG_MODE
  TC_UBYTE, //IO_OUT_OVERLOAD
  TC_UBYTE, //IO_OUT_REG_P_VAL
  TC_UBYTE, //IO_OUT_REG_I_VAL
  TC_UBYTE, //IO_OUT_REG_D_VAL
  TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
  TC_SLONG, //IO_OUT_ROTATION_COUNT
};


TYPE_CODE * IO_TYPES[2] =
{
  IO_TYPES_IN,
  IO_TYPES_OUT
};

//Actual pointers filled in during cCmdInit()
void * IO_PTRS_IN[IO_IN_FIELD_COUNT];
void * IO_PTRS_OUT[IO_OUT_FIELD_COUNT];

void ** IO_PTRS[2] =
{
  IO_PTRS_IN,
  IO_PTRS_OUT
};


//cCmdHandleRemoteCommands is the registered handler for "direct" command protocol packets
//It is only intended to be called via c_comm's main protocol handler
UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
{
  NXT_STATUS RCStatus = NO_ERR;
  //Response packet length.  Always includes RCStatus byte.
  ULONG ResponseLen = 1;
  //Boolean flag to send a response.  TRUE unless overridden below.
  ULONG SendResponse = TRUE;
  //Boolean flag if we are handling a reply telegram.  FALSE unless overridden.
  ULONG IncomingReply = FALSE;
  ULONG i, FirstPort, LastPort;
  UWORD LStatus;
  UWORD Count, QueueID;
  UBYTE * pData;

  //Illegal call, give up
  if (pInBuf == NULL || pLen == NULL)
  {
    NXT_BREAK;
    return (0xFFFF);
  }

  //No output buffer provided, so skip any work related to returning a response
  if (pOutBuf == NULL)
    SendResponse = FALSE;

  //If first byte identifies this as a reply telegram, we have different work to do.
  if (pInBuf[0] == 0x02)
  {
    IncomingReply = TRUE;
    //Reply telegrams never get responses, even if caller provided a buffer.
    SendResponse = FALSE;
  }

  //Advance pInBuf past command type byte
  pInBuf++;

  if (!IncomingReply)
  {
    switch(pInBuf[0])
    {
      case RC_START_PROGRAM:
      {
        //Check that file exists.  If not, return error
        //!!! Should return standard loader file error in cases like this??
        //!!! Proper solution would also check file mode to avoid confusing errors
        if (LOADER_ERR(LStatus = pMapLoader->pFunc(FINDFIRST, (&pInBuf[1]), NULL, NULL)) != SUCCESS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //Close file handle returned by FINDFIRST
        pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(LStatus), NULL, NULL);

        //File must exist, so inform UI to attempt execution in the usual way (enables consistent feedback)
        pMapUi->Flags |= UI_EXECUTE_LMS_FILE;
        strncpy((PSZ)(pMapUi->LMSfilename), (PSZ)(&pInBuf[1]), FILENAME_LENGTH + 1);
      }
      break;

      case RC_STOP_PROGRAM:
      {
        if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
        {
          RCStatus = ERR_NO_PROG;
          break;
        }

        IOMapCmd.DeactivateFlag = TRUE;
      }
      break;

      case RC_PLAY_SOUND_FILE:
      {
        if (LOADER_ERR(pMapLoader->pFunc(FINDFIRST, (&pInBuf[2]), NULL, NULL)) != SUCCESS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //Close file handle returned by FINDFIRST
        pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(LStatus), NULL, NULL);

        if (pInBuf[1] == FALSE)
          pMapSound->Mode = SOUND_ONCE;
        else //Any non-zero value treated as TRUE
          pMapSound->Mode = SOUND_LOOP;

        strncpy((PSZ)pMapSound->SoundFilename, (PSZ)(&pInBuf[2]), FILENAME_LENGTH + 1);
        pMapSound->Flags |= SOUND_UPDATE;
      }
      break;

      case RC_PLAY_TONE:
      {
        pMapSound->Mode = SOUND_TONE;
        //!!! Range check valid values?
        memcpy((PSZ)(&(pMapSound->Freq)), (PSZ)(&pInBuf[1]), 2);
        memcpy((PSZ)(&(pMapSound->Duration)), (PSZ)(&pInBuf[3]), 2);

        pMapSound->Flags |= SOUND_UPDATE;
      }
      break;

      case RC_SET_OUT_STATE:
      {
        //Don't do anything if illegal port specification is made
        if (pInBuf[1] >= NO_OF_OUTPUTS && pInBuf[1] != 0xFF)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //0xFF is protocol defined to mean "all ports".
        if (pInBuf[1] == 0xFF)
        {
          FirstPort = 0;
          LastPort = NO_OF_OUTPUTS - 1;
        }
        else
          FirstPort = LastPort = pInBuf[1];

        for (i = FirstPort; i <= LastPort; i++)
        {
          pMapOutPut->Outputs[i].Speed             = pInBuf[2];
          pMapOutPut->Outputs[i].Mode              = pInBuf[3];
          pMapOutPut->Outputs[i].RegMode           = pInBuf[4];
          pMapOutPut->Outputs[i].SyncTurnParameter = pInBuf[5];
          pMapOutPut->Outputs[i].RunState          = pInBuf[6];
          memcpy((PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), (PSZ)(&pInBuf[7]), 4);

          pMapOutPut->Outputs[i].Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT;
        }
      }
      break;

      case RC_SET_IN_MODE:
      {
        i = pInBuf[1];

        //Don't do anything if illegal port specification is made
        //!!! Should check against legal Types and Modes? (bitmask for Modes?)
        if (i >= NO_OF_INPUTS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        pMapInput->Inputs[i].SensorType = pInBuf[2];
        pMapInput->Inputs[i].SensorMode = pInBuf[3];

        //Set InvalidData flag automatically since type may have changed
        pMapInput->Inputs[i].InvalidData = TRUE;
      }
      break;

      case RC_GET_OUT_STATE:
      {
        if (SendResponse == TRUE)
        {
          i = pInBuf[1];

          //Return error and all zeros if illegal port specification is made
          if (i >= NO_OF_OUTPUTS)
          {
            RCStatus = ERR_RC_ILLEGAL_VAL;
            memset(&(pOutBuf[ResponseLen]), 0, 22);
            ResponseLen += 22;
            break;
          }

          //Echo port
          pOutBuf[ResponseLen] = i;
          ResponseLen++;

          //Power
          pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Speed;
          ResponseLen++;

          //Mode
          pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Mode;
          ResponseLen++;

          //RegMode
          pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RegMode;
          ResponseLen++;

          //TurnRatio
          pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].SyncTurnParameter;
          ResponseLen++;

          //RunState
          pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RunState;
          ResponseLen++;

          //TachoLimit ULONG
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), 4);
          ResponseLen += 4;

          //TachoCount SLONG