c_input.c

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

      switch (VarsInput.LastAngle[Tmp])
      {
        case 0 :
        {
          if (Sample == 1)
          {
            if (VarsInput.SampleCnt[Tmp] >= ROT_SLOW_SPEED )
            {

              if (++(VarsInput.SampleCnt[Tmp]) >= (ROT_SLOW_SPEED + ROT_OV_SAMPLING))
              {
                (IOMapInput.Inputs[Tmp].SensorValue)++;
                VarsInput.LastAngle[Tmp] = Sample;
              }
            }
            else
            {
              (IOMapInput.Inputs[Tmp].SensorValue)++;
              VarsInput.LastAngle[Tmp] = Sample;
            }
          }
          if (Sample == 2)
          {
            (IOMapInput.Inputs[Tmp].SensorValue)--;
            VarsInput.LastAngle[Tmp] = Sample;
          }
          if (Sample == 0)
          {
            if (VarsInput.SampleCnt[Tmp] < ROT_SLOW_SPEED)
            {
              (VarsInput.SampleCnt[Tmp])++;
            }
          }
        }
        break;
        case 1 :
        {
          if (Sample == 3)
          {
            (IOMapInput.Inputs[Tmp].SensorValue)++;
            VarsInput.LastAngle[Tmp] = Sample;
          }
          if (Sample == 0)
          {
            (IOMapInput.Inputs[Tmp].SensorValue)--;
            VarsInput.LastAngle[Tmp] = Sample;
          }
          VarsInput.SampleCnt[Tmp] = 0;
        }
        break;
        case 2 :
        {
          if (Sample == 0)
          {
            (IOMapInput.Inputs[Tmp].SensorValue)++;
            VarsInput.LastAngle[Tmp] = Sample;
          }
          if (Sample == 3)
          {
            (IOMapInput.Inputs[Tmp].SensorValue)--;
            VarsInput.LastAngle[Tmp] = Sample;
          }
          VarsInput.SampleCnt[Tmp] = 0;
        }
        break;
        case 3 :
        {
          if (Sample == 2)
          {
            if (VarsInput.SampleCnt[Tmp] >= ROT_SLOW_SPEED)
            {

              if (++(VarsInput.SampleCnt[Tmp]) >= (ROT_SLOW_SPEED + ROT_OV_SAMPLING))
              {
                (IOMapInput.Inputs[Tmp].SensorValue)++;
                VarsInput.LastAngle[Tmp] = Sample;
              }
            }
            else
            {
              (IOMapInput.Inputs[Tmp].SensorValue)++;
              VarsInput.LastAngle[Tmp] = Sample;
            }
          }
          if (Sample == 1)
          {
            (IOMapInput.Inputs[Tmp].SensorValue)--;
             VarsInput.LastAngle[Tmp] = Sample;
          }
          if (Sample == 3)
          {
            if (VarsInput.SampleCnt[Tmp] < ROT_SLOW_SPEED)
            {
              (VarsInput.SampleCnt[Tmp])++;
            }
          }
        }
        break;
      }
    }
  }

  IOMapInput.Inputs[Tmp].SensorBoolean  = PresentBoolean;
}


const SWORD TempConvTable[] =
{
  1500, 1460, 1430, 1400, 1380, 1360, 1330, 1310, 1290, 1270, 1250, 1230, 1220, 1200, 1190, 1170,
  1160, 1150, 1140, 1130, 1110, 1100, 1090, 1080, 1070, 1060, 1050, 1040, 1030, 1020, 1010, 1000,
   994,  988,  982,  974,  968,  960,  954,  946,  940,  932,  926,  918,  912,  906,  900,  894,
   890,  884,  878,  874,  868,  864,  858,  854,  848,  844,  838,  832,  828,  822,  816,  812,
   808,  802,  798,  794,  790,  786,  782,  780,  776,  772,  768,  764,  762,  758,  754,  750,
   748,  744,  740,  736,  732,  730,  726,  722,  718,  716,  712,  708,  704,  700,  696,  694,
   690,  688,  684,  682,  678,  674,  672,  668,  666,  662,  660,  656,  654,  650,  648,  644,
   642,  640,  638,  634,  632,  630,  628,  624,  622,  620,  616,  614,  612,  610,  608,  604,
   602,  600,  598,  596,  592,  590,  588,  586,  584,  582,  580,  578,  576,  574,  572,  570,
   568,  564,  562,  560,  558,  556,  554,  552,  550,  548,  546,  544,  542,  540,  538,  536,
   534,  532,  530,  528,  526,  524,  522,  520,  518,  516,  514,  512,  510,  508,  508,  506,
   504,  502,  500,  498,  496,  494,  494,  492,  490,  488,  486,  486,  484,  482,  480,  478,
   476,  476,  474,  472,  470,  468,  468,  466,  464,  462,  460,  458,  458,  456,  454,  452,
   450,  448,  448,  446,  444,  442,  442,  440,  438,  436,  436,  434,  432,  432,  430,  428,
   426,  426,  424,  422,  420,  420,  418,  416,  416,  414,  412,  410,  408,  408,  406,  404,
   404,  402,  400,  398,  398,  396,  394,  394,  392,  390,  390,  388,  386,  386,  384,  382,
   382,  380,  378,  378,  376,  374,  374,  372,  370,  370,  368,  366,  366,  364,  362,  362,
   360,  358,  358,  356,  354,  354,  352,  350,  350,  348,  348,  346,  344,  344,  342,  340,
   340,  338,  338,  336,  334,  334,  332,  332,  330,  328,  328,  326,  326,  324,  322,  322,
   320,  320,  318,  316,  316,  314,  314,  312,  310,  310,  308,  308,  306,  304,  304,  302,
   300,  300,  298,  298,  296,  296,  294,  292,  292,  290,  290,  288,  286,  286,  284,  284,
   282,  282,  280,  280,  278,  278,  276,  274,  274,  272,  272,  270,  270,  268,  268,  266,
   264,  264,  262,  262,  260,  260,  258,  258,  256,  254,  254,  252,  252,  250,  250,  248,
   248,  246,  244,  244,  242,  240,  240,  240,  238,  238,  236,  236,  234,  234,  232,  230,
   230,  228,  228,  226,  226,  224,  224,  222,  220,  220,  218,  218,  216,  216,  214,  214,
   212,  212,  210,  210,  208,  208,  206,  204,  204,  202,  202,  200,  200,  198,  198,  196,
   196,  194,  194,  192,  190,  190,  188,  188,  186,  186,  184,  184,  182,  182,  180,  180,
   178,  178,  176,  176,  174,  174,  172,  172,  170,  170,  168,  168,  166,  166,  164,  164,
   162,  162,  160,  160,  158,  156,  156,  154,  154,  152,  152,  150,  150,  148,  148,  146,
   146,  144,  144,  142,  142,  140,  140,  138,  136,  136,  136,  134,  134,  132,  130,  130,
   128,  128,  126,  126,  124,  124,  122,  122,  120,  120,  118,  118,  116,  116,  114,  114,
   112,  110,  110,  108,  108,  106,  106,  104,  104,  102,  102,  100,  100,  98,  98,  96,
   94,  94,  92,  92,  90,  90,  88,  88,  86,  86,  84,  82,  82,  80,  80,  78,
   78,  76,  76,  74,  74,  72,  72,  70,  70,  68,  68,  66,  66,  64,  62,  62,
   60,  60,  58,  56,  56,  54,  54,  52,  52,  50,  50,  48,  48,  46,  46,  44,
   44,  42,  40,  40,  38,  38,  36,  34,  34,  32,  32,  30,  30,  28,  28,  26,
   24,  24,  22,  22,  20,  20,  18,  16,  16,  14,  14,  12,  12,  10,  10,  8,
   6,  6,  4,  2,  2,  0,  0,  -2,  -4,  -4,  -6,  -6,  -8,  -10,  -10,  -12,
   -12,  -14,  -16,  -16,  -18,  -20,  -20,  -22,  -22,  -24,  -26,  -26,  -28,  -30,  -30,  -32,
   -34,  -34,  -36,  -36,  -38,  -40,  -40,  -42,  -42,  -44,  -46,  -46,  -48,  -50,  -50,  -52,
   -54,  -54,  -56,  -58,  -58,  -60,  -60,  -62,  -64,  -66,  -66,  -68,  -70,  -70,  -72,  -74,
   -76,  -76,  -78,  -80,  -80,  -82,  -84,  -86,  -86,  -88,  -90,  -90,  -92,  -94,  -94,  -96,
   -98,  -98,  -100,  -102,  -104,  -106,  -106,  -108,  -110,  -112,  -114,  -114,  -116,  -118,  -120,  -120,
   -122,  -124,  -126,  -128,  -130,  -130,  -132,  -134,  -136,  -138,  -140,  -142,  -144,  -146,  -146,  -148,
   -150,  -152,  -154,  -156,  -158,  -160,  -162,  -164,  -166,  -166,  -168,  -170,  -172,  -174,  -176,  -178,
   -180,  -182,  -184,  -186,  -188,  -190,  -192,  -194,  -196,  -196,  -198,  -200,  -202,  -204,  -206,  -208,
   -210,  -212,  -214,  -216,  -218,  -220,  -224,  -226,  -228,  -230,  -232,  -234,  -236,  -238,  -242,  -246,
   -248,  -250,  -254,  -256,  -260,  -262,  -264,  -268,  -270,  -274,  -276,  -278,  -282,  -284,  -286,  -290,
   -292,  -296,  -298,  -300,  -306,  -308,  -312,  -316,  -320,  -324,  -326,  -330,  -334,  -338,  -342,  -344,
   -348,  -354,  -358,  -362,  -366,  -370,  -376,  -380,  -384,  -388,  -394,  -398,  -404,  -410,  -416,  -420,
   -428,  -432,  -440,  -446,  -450,  -460,  -468,  -476,  -484,  -492,  -500,  -510,  -524,  -534,  -546,  -560,
   -572,  -588,  -600,  -630,  -656,  -684,  -720,  -770
};


void      cInputCalcSensorRaw(UWORD *pRaw, UBYTE Type, UBYTE No)
{

  switch (Type)
  {
    case SWITCH:
    {
    }
    break;
    case TEMPERATURE:
    {
      if (*pRaw < 290)
      {
        *pRaw = 290;
      }
      else
      {
        if (*pRaw > 928)
        {
          *pRaw = 928;
        }
      }
      *pRaw = TempConvTable[(*pRaw) - 197];
      *pRaw = *pRaw + 200;
      *pRaw = (UWORD)(((SLONG)*pRaw * (SLONG)1023)/(SLONG)900);
    }
    break;
    case REFLECTION:
    {

      /* Sensor dynanmic is restricted by a double diode connected to ground,  */
      /* and it cannot go to the top either, dynamic is approx. 390 - 900 count*/
       cInputCalcFullScale(pRaw, REFLECTIONSENSORMIN, REFLECTIONSENSORPCTDYN, TRUE);
    }
    break;
    case ANGLE:
    {
    }
    break;
    case LIGHT_ACTIVE:
    {
      cInputCalcFullScale(pRaw, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE);
    }
    break;
    case LIGHT_INACTIVE:
    {
      cInputCalcFullScale(pRaw, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE);
    }
    break;
    case SOUND_DB:
    {
      cInputCalcFullScale(pRaw, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE);
    }
    break;
    case SOUND_DBA:
    {
      cInputCalcFullScale(pRaw, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE);
    }
    break;
    case LOWSPEED:
    {
      /* Intended empty Low Speed module takes over     */
    }
    break;
    case HIGHSPEED:
    {
    }
    break;
    case CUSTOM:
    {

      /* Setup and read digital IO */
      cInputSetupCustomSensor(No);
      dInputRead0(No, &(IOMapInput.Inputs[No].DigiPinsIn));
      dInputRead1(No, &(IOMapInput.Inputs[No].DigiPinsIn));
      cInputCalcFullScale(pRaw, IOMapInput.Inputs[No].CustomZeroOffset, IOMapInput.Inputs[No].CustomPctFullScale, FALSE);
    }
    break;
    case NO_SENSOR:
    {
    }
    break;
    default:
    {
    }
    break;
  }
}

void      cInputCalcFullScale(UWORD *pRawVal, UWORD ZeroPointOffset, UBYTE PctFullScale, UBYTE InvStatus)
{
  if (*pRawVal >= ZeroPointOffset)
  {
    *pRawVal -= ZeroPointOffset;
  }
  else
  {
    *pRawVal = 0;
  }

  *pRawVal = (*pRawVal * 100)/PctFullScale;
  if (*pRawVal > SENSOR_RESOLUTION)
  {
    *pRawVal = SENSOR_RESOLUTION;
  }
  if (TRUE == InvStatus)
  {
    *pRawVal = SENSOR_RESOLUTION - *pRawVal;
  }
}

void      cInputSetupType(UBYTE Port)
{
  UBYTE   Setup;

  Setup = (ActiveList[IOMapInput.Inputs[Port].SensorType]);

  if (CUSTOM_SETUP & Setup)
  {
    cInputSetupCustomSensor(Port);
  }
  else
  {
    if (NO_POWER & Setup)
    {

      /* Setup is not used - set pins in unconfigured state */
      dInputSetInactive(Port);
      dInputSetDirInDigi0(Port);
      dInputSetDirInDigi1(Port);
    }
    else
    {
      if (ACTIVE & Setup)
      {
        dInputSetActive(Port);
      }
      else
      {
        if(ALWAYS_ACTIVE & Setup)
        {
          dInputSet9v(Port);
        }
        else
        {
          dInputSetInactive(Port);
        }
      }
      if (DIGI_0_HIGH & Setup)
      {
        dInputSetDigi0(Port);
        dInputSetDirOutDigi0(Port);
      }
      else
      {
        dInputClearDigi0(Port);
        dInputSetDirOutDigi0(Port);
      }

      if (DIGI_1_HIGH & Setup)
      {
        dInputSetDigi1(Port);
        dInputSetDirOutDigi1(Port);
      }
      else
      {
        dInputClearDigi1(Port);
        dInputSetDirOutDigi1(Port);
      }
    }
  }
}

void      cInputSetupCustomSensor(UBYTE Port)
{
  if ((IOMapInput.Inputs[Port].DigiPinsDir) & 0x01)
  {
    if ((IOMapInput.Inputs[Port].DigiPinsOut) & 0x01)
    {
      dInputSetDigi0(Port);
    }
    else
    {
      dInputClearDigi0(Port);
    }
    dInputSetDirOutDigi0(Port);
  }
  if ((IOMapInput.Inputs[Port].DigiPinsDir) & 0x02)
  {
    if ((IOMapInput.Inputs[Port].DigiPinsOut) & 0x02)
    {
      dInputSetDigi1(Port);
    }
    else
    {
      dInputClearDigi1(Port);
    }
    dInputSetDirOutDigi1(Port);
  }
  else
  {
    dInputSetDirInDigi1(Port);
  }

  if (CUSTOMACTIVE == (IOMapInput.Inputs[Port].CustomActiveStatus))
  {
    dInputSetActive(Port);
  }
  else
  {
    if (CUSTOM9V == (IOMapInput.Inputs[Port].CustomActiveStatus))
    {
      dInputSet9v(Port);
    }
    else
    {
      dInputSetInactive(Port);
    }
  }
}

void      cInputExit(void)
{
  dInputExit();
}