c_input.c

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

//
// Date init       14.12.2004
//
// Revision date   $Date:: 16-05-06 10:06                                    $
//
// Filename        $Workfile:: c_input.c                                     $
//
// Version         $Revision:: 25                                            $
//
// Archive         $Archive:: /LMS2006/Sys01/Main/Firmware/Source/c_input.c  $
//
// Platform        C
//

#include  "stdconst.h"
#include  "modules.h"
#include  "c_input.h"
#include  "d_input.h"
#include  "c_output.iom"

#define   INVALID_RELOAD_NORMAL         20
#define   INVALID_RELOAD_SOUND          300

#define   ROT_SLOW_SPEED                30
#define   ROT_OV_SAMPLING               7

#define   VCC_SENSOR                    5000L
#define   VCC_SENSOR_DIODE              4300L
#define   AD_MAX                        1023L

#define   REFLECTIONSENSORMIN           (1906L/(VCC_SENSOR/AD_MAX))
#define   REFLECTIONSENSORMAX           ((AD_MAX * 4398L)/VCC_SENSOR)
#define   REFLECTIONSENSORPCTDYN        (UBYTE)(((REFLECTIONSENSORMAX - REFLECTIONSENSORMIN) * 100L)/AD_MAX)

#define   NEWLIGHTSENSORMIN             (800L/(VCC_SENSOR/AD_MAX))
#define   NEWLIGHTSENSORMAX             ((AD_MAX * 4400L)/VCC_SENSOR)
#define   NEWLIGHTSENSORPCTDYN          (UBYTE)(((NEWLIGHTSENSORMAX - NEWLIGHTSENSORMIN) * 100L)/AD_MAX)

#define   NEWSOUNDSENSORMIN             (650L/(VCC_SENSOR/AD_MAX))
#define   NEWSOUNDSENSORMAX             ((AD_MAX * 4980L)/VCC_SENSOR)
#define   NEWSOUNDSENSORPCTDYN          (UBYTE)(((NEWSOUNDSENSORMAX - NEWSOUNDSENSORMIN) * 100L)/AD_MAX)

enum
{
  POWER         = 0x00,
  NO_POWER      = 0x01,
  ACTIVE        = 0x02,
  ALWAYS_ACTIVE = 0x04,
  DIGI_0_HIGH   = 0x08,
  DIGI_1_HIGH   = 0x10,
  CUSTOM_SETUP  = 0x20
};

static const UBYTE ActiveList[NO_OF_SENSOR_TYPES] =
{
  NO_POWER,                                     /* NO_SENSOR        */
  NO_POWER,                                     /* SWITCH           */
  NO_POWER,                                     /* TEMPERATURE      */
  ACTIVE,                                       /* REFLECTION       */
  ACTIVE,                                       /* ANGLE            */
  DIGI_0_HIGH,                                  /* LIGHT_ACTIVE     */
  POWER,                                        /* LIGHT_INACTIVE   */
  DIGI_0_HIGH,                                  /* SOUND_DB         */
  DIGI_1_HIGH,                                  /* SOUND_DBA        */
  CUSTOM_SETUP,                                 /* CUSTOM           */
  DIGI_0_HIGH | DIGI_1_HIGH,                    /* LOWSPEED         */
  ALWAYS_ACTIVE | DIGI_0_HIGH | DIGI_1_HIGH     /* LOWSPEED_9V on   */

};

static    IOMAPINPUT   IOMapInput;
static    VARSINPUT    VarsInput;

const     HEADER       cInput =
{
  0x00030001L,
  "Input",
  cInputInit,
  cInputCtrl,
  cInputExit,
  (void *)&IOMapInput,
  (void *)&VarsInput,
  (UWORD)sizeof(IOMapInput),
  (UWORD)sizeof(VarsInput),
  0x0000                      //Code size - not used so far
};

void      cInputCalcSensorRaw(UWORD *pRaw, UBYTE Type, UBYTE No);
void      cInputCalcFullScale(UWORD *pRawVal, UWORD ZeroPointOffset, UBYTE PctFullScale, UBYTE InvState);
void      cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp);
void      cInputCalcSensor(UBYTE Tmp);
void      cInputSetupType(UBYTE Port);
void      cInputSetupCustomSensor(UBYTE Port);


void      cInputInit(void* pHeader)
{
  UBYTE   Tmp;

  /* Init IO map */
  for (Tmp = 0; Tmp < NO_OF_INPUTS; Tmp++)
  {
    IOMapInput.Inputs[Tmp].SensorType         = NO_SENSOR;
    IOMapInput.Inputs[Tmp].SensorMode         = RAWMODE;
    IOMapInput.Inputs[Tmp].SensorRaw          = 0;
    IOMapInput.Inputs[Tmp].SensorValue        = 0;
    IOMapInput.Inputs[Tmp].SensorBoolean      = 0;
    IOMapInput.Inputs[Tmp].InvalidData        = INVALID_DATA;
    IOMapInput.Inputs[Tmp].DigiPinsDir        = 0;
    IOMapInput.Inputs[Tmp].DigiPinsOut        = 0;
    IOMapInput.Inputs[Tmp].CustomActiveStatus = CUSTOMINACTIVE;
    IOMapInput.Inputs[Tmp].CustomZeroOffset   = 0;
    IOMapInput.Inputs[Tmp].CustomPctFullScale = 0;
    dInputRead0(Tmp, &(IOMapInput.Inputs[Tmp].DigiPinsIn));
    dInputRead1(Tmp, &(IOMapInput.Inputs[Tmp].DigiPinsIn));

    VarsInput.EdgeCnt[Tmp]       = 0;
    VarsInput.InputDebounce[Tmp] = 0;
    VarsInput.LastAngle[Tmp]     = 0;
    VarsInput.SampleCnt[Tmp]     = 0;
    VarsInput.InvalidTimer[Tmp]  = INVALID_RELOAD_NORMAL;
    VarsInput.OldSensorType[Tmp] = NO_SENSOR;
  }
  dInputInit();
}

void      cInputCtrl(void)
{
  UBYTE   Tmp;

  for (Tmp = 0; Tmp < NO_OF_INPUTS; Tmp++)
  {

    if ((IOMapInput.Inputs[Tmp].SensorType) != (VarsInput.OldSensorType[Tmp]))
    {

      /* Clear all variables for this sensor */
      VarsInput.EdgeCnt[Tmp]       = 0;
      VarsInput.InputDebounce[Tmp] = 0;
      VarsInput.LastAngle[Tmp]     = 0;
      VarsInput.SampleCnt[Tmp]     = 0;

      /* Has the type change any influence on pin setup? */
      if ((ActiveList[IOMapInput.Inputs[Tmp].SensorType]) != (ActiveList[VarsInput.OldSensorType[Tmp]]))
      {

        /* The type change has influence on pin setup - therefore a delay is inserted    */
        /* to ensure valid data representation (delay is: time for data to get down into */
        /* the avr + delay before ad conversion + time for data to get up to the ARM     */
        /* processor                                                                     */
        if ((SOUND_DB == IOMapInput.Inputs[Tmp].SensorType) || (SOUND_DBA == IOMapInput.Inputs[Tmp].SensorType))
        {

          /* Sound Sensors have a long hardware stabilizing time */
          VarsInput.InvalidTimer[Tmp] = INVALID_RELOAD_SOUND;
        }
        else
        {
          VarsInput.InvalidTimer[Tmp] = INVALID_RELOAD_NORMAL;
        }

        /* Setup the pins for the new sensortype */
        cInputSetupType(Tmp);
        IOMapInput.Inputs[Tmp].InvalidData  = INVALID_DATA;
      }
      VarsInput.OldSensorType[Tmp] = IOMapInput.Inputs[Tmp].SensorType;
    }
    else
    {
      if (VarsInput.InvalidTimer[Tmp])
      {

        /* A type change has bee carried out earlier - waiting for valid data   */
        (VarsInput.InvalidTimer[Tmp])--;
        if (0 == VarsInput.InvalidTimer[Tmp])
        {

          /* Time elapsed - data are now valid */
          IOMapInput.Inputs[Tmp].InvalidData &= ~INVALID_DATA;
        }
      }
      else
      {

        /* The invalid bit could have been set by the VM due to Mode change    */
        /* but input module needs to be called once to update the values       */
        IOMapInput.Inputs[Tmp].InvalidData &= ~INVALID_DATA;
      }
    }

    if (!(INVALID_DATA & (IOMapInput.Inputs[Tmp].InvalidData)))
    {
      cInputCalcSensor(Tmp);
    }
  }
}


void      cInputCalcSensor(UBYTE Tmp)
{
  UWORD   InputRaw;

  /* Get the RCX hardware compensated AD values put values in     */
  /* InputRaw array                                               */
  dInputGetRawAd(&InputRaw, Tmp);
  IOMapInput.Inputs[Tmp].ADRaw = InputRaw;

  /* Calculate the sensor hardware compensated AD values and put then */
  /* in IOMapInput.Inputs[Tmp].SensorRaw                              */
  cInputCalcSensorRaw(&InputRaw, IOMapInput.Inputs[Tmp].SensorType, Tmp);

  /* Calculate the sensor value compensated for sensor mode and put   */
  /* them in IOMapInput.Inputs[Tmp].SensorValue                       */
  cInputCalcSensorValue( &InputRaw,
                        ((IOMapInput.Inputs[Tmp].SensorMode) & SLOPEMASK),
                        ((IOMapInput.Inputs[Tmp].SensorMode) & MODEMASK),
                          Tmp);

}



void      cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
{
  SWORD   Delta;
  UBYTE   PresentBoolean;
  UBYTE   Sample;

  if (0 == Slope)
  {

    /* This is absolute measure method */
    if (*pRaw > THRESHOLD_FALSE)
    {
      PresentBoolean = FALSE;
    }
    else
    {
      if (*pRaw < THRESHOLD_TRUE)
      {
        PresentBoolean = TRUE;
      }
    }
  }
  else
  {

    /* This is dynamic measure method */
    if (*pRaw > (ACTUAL_AD_RES - Slope))
    {
      PresentBoolean = FALSE;
    }
    else
    {
      if (*pRaw < Slope)
      {
        PresentBoolean = TRUE;
      }
      else
      {
        Delta = IOMapInput.Inputs[Tmp].SensorRaw - *pRaw;
        if (Delta < 0)
        {
          if (-Delta > Slope)
          {
            PresentBoolean = FALSE;
          }
        }
        else
        {
          if (Delta > Slope)
          {
            PresentBoolean = TRUE;
          }
        }
      }
    }
  }
  IOMapInput.Inputs[Tmp].SensorRaw = *pRaw;

  switch(Mode)
  {

    case RAWMODE:
    {
      IOMapInput.Inputs[Tmp].SensorValue   = *pRaw;
    }
    break;

    case BOOLEANMODE:
    {
      IOMapInput.Inputs[Tmp].SensorValue    = PresentBoolean;
    }
    break;

    case TRANSITIONCNTMODE:
    {
      if (VarsInput.InputDebounce[Tmp] > 0)
      {
        VarsInput.InputDebounce[Tmp]--;
      }
      else
      {
        if (IOMapInput.Inputs[Tmp].SensorBoolean != PresentBoolean)
        {
          VarsInput.InputDebounce[Tmp] = DEBOUNCERELOAD;
          (IOMapInput.Inputs[Tmp].SensorValue)++;
        }
      }
    }
    break;

    case PERIODCOUNTERMODE:
    {
      if (VarsInput.InputDebounce[Tmp] > 0)
      {
        VarsInput.InputDebounce[Tmp]--;
      }
      else
      {
        if (IOMapInput.Inputs[Tmp].SensorBoolean != PresentBoolean)
        {
          VarsInput.InputDebounce[Tmp] = DEBOUNCERELOAD;
          IOMapInput.Inputs[Tmp].SensorBoolean = PresentBoolean;
          if (++VarsInput.EdgeCnt[Tmp] > 1)
          {
            if (PresentBoolean == 0)
            {
              VarsInput.EdgeCnt[Tmp] = 0;
              (IOMapInput.Inputs[Tmp].SensorValue)++;
            }
          }
        }
      }
    }
    break;

    case PCTFULLSCALEMODE:
    {

      /* Output is 0-100 pct */
      IOMapInput.Inputs[Tmp].SensorValue   = ((*pRaw) * 100)/SENSOR_RESOLUTION;
    }
    break;

    case FAHRENHEITMODE:
    {

      /* Fahrenheit mode goes from -40 to 158 degrees */
      IOMapInput.Inputs[Tmp].SensorValue   = (((ULONG)(*pRaw) * 900L)/SENSOR_RESOLUTION) - 200;
      IOMapInput.Inputs[Tmp].SensorValue   =  ((180L * (ULONG)(IOMapInput.Inputs[Tmp].SensorValue))/100L) + 320;
    }
    break;

    case CELSIUSMODE:
    {

      /* Celsius mode goes from -20 to 70 degrees */
      IOMapInput.Inputs[Tmp].SensorValue   = (((ULONG)(*pRaw) * 900L)/SENSOR_RESOLUTION) - 200;
    }
    break;

    case ANGLESTEPSMODE:
    {
      IOMapInput.Inputs[Tmp].SensorBoolean = PresentBoolean;

      if (*pRaw < ANGLELIMITA)
      {
        Sample = 0;
      }
      else
      {
        if (*pRaw < ANGLELIMITB)
        {
          Sample = 1;
        }
        else
        {
          if (*pRaw < ANGLELIMITC)
          {
            Sample = 2;
          }
          else
          {
            Sample = 3;
          }
        }
      }