summaryrefslogtreecommitdiff
path: root/AT91SAM7S256/Source/c_input.c
diff options
context:
space:
mode:
Diffstat (limited to 'AT91SAM7S256/Source/c_input.c')
-rw-r--r--AT91SAM7S256/Source/c_input.c1326
1 files changed, 1036 insertions, 290 deletions
diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c
index d17c546..47ca7c0 100644
--- a/AT91SAM7S256/Source/c_input.c
+++ b/AT91SAM7S256/Source/c_input.c
@@ -1,13 +1,14 @@
+
//
// Date init 14.12.2004
//
-// Revision date $Date:: 16-05-06 10:06 $
+// Revision date $Date:: 3/21/09 10:31a $
//
// Filename $Workfile:: c_input.c $
//
-// Version $Revision:: 25 $
+// Version $Revision:: 39 $
//
-// Archive $Archive:: /LMS2006/Sys01/Main/Firmware/Source/c_input.c $
+// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_inpu $
//
// Platform C
//
@@ -17,9 +18,13 @@
#include "c_input.h"
#include "d_input.h"
#include "c_output.iom"
+#include "c_loader.iom"
+#include <string.h>
+
#define INVALID_RELOAD_NORMAL 20
#define INVALID_RELOAD_SOUND 300
+#define INVALID_RELOAD_COLOR 400
#define ROT_SLOW_SPEED 30
#define ROT_OV_SAMPLING 7
@@ -40,6 +45,14 @@
#define NEWSOUNDSENSORMAX ((AD_MAX * 4980L)/VCC_SENSOR)
#define NEWSOUNDSENSORPCTDYN (UBYTE)(((NEWSOUNDSENSORMAX - NEWSOUNDSENSORMIN) * 100L)/AD_MAX)
+/* Remember this is ARM AD converter - 3,3 VDC as max voltage */
+/* When in color mode background value is substracted => min = 0!!! */
+#define COLORSENSORBGMIN (214/(3300/AD_MAX))
+#define COLORSENSORMIN (1L/(3300/AD_MAX)) /* 1 inserted else div 0 (1L/(120/AD_MAX)) */
+#define COLORSENSORMAX ((AD_MAX * 3300L)/3300)
+#define COLORSENSORPCTDYN (UBYTE)(((COLORSENSORMAX - COLORSENSORMIN) * 100L)/AD_MAX)
+#define COLORSENSORBGPCTDYN (UBYTE)(((COLORSENSORMAX - COLORSENSORBGMIN) * 100L)/AD_MAX)
+
enum
{
POWER = 0x00,
@@ -48,24 +61,66 @@ enum
ALWAYS_ACTIVE = 0x04,
DIGI_0_HIGH = 0x08,
DIGI_1_HIGH = 0x10,
- CUSTOM_SETUP = 0x20
+ DIGI_0_IN = 0x20,
+ DIGI_1_IN = 0x40,
+ CUSTOM_SETUP = 0x80
};
-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 */
+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
};
static IOMAPINPUT IOMapInput;
@@ -85,18 +140,24 @@ const HEADER cInput =
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 cInputCalcSensorValue(UWORD NewSensorRaw, UWORD *pOldSensorRaw, SWORD *pSensorValue,
+ UBYTE *pBoolean, UBYTE *pDebounce, UBYTE *pSampleCnt,
+ UBYTE *LastAngle, UBYTE *pEdgeCnt, UBYTE Slope,
+ UBYTE Mode);
+void cInputSetupType(UBYTE Port, UBYTE *pType, UBYTE OldType);
void cInputSetupCustomSensor(UBYTE Port);
-
+void cInputCalcSensorValues(UBYTE No);
+UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus);
+void cInputCalibrateColor(COLORSTRUCT *pC, UWORD *pNewVals);
void cInputInit(void* pHeader)
{
UBYTE Tmp;
+ memset(IOMapInput.Colors, 0, sizeof(IOMapInput.Colors));
+ memset(VarsInput.VarsColor, 0, sizeof(VarsInput.VarsColor));
+
/* Init IO map */
for (Tmp = 0; Tmp < NO_OF_INPUTS; Tmp++)
{
@@ -121,6 +182,10 @@ void cInputInit(void* pHeader)
VarsInput.InvalidTimer[Tmp] = INVALID_RELOAD_NORMAL;
VarsInput.OldSensorType[Tmp] = NO_SENSOR;
}
+
+ VarsInput.ColorStatus = 0;
+ VarsInput.ColorCnt = 0;
+
dInputInit();
}
@@ -128,6 +193,37 @@ void cInputCtrl(void)
{
UBYTE Tmp;
+
+ if (VarsInput.ColorStatus)
+ {
+ switch(VarsInput.ColorCnt)
+ {
+ case 0:
+ {
+ VarsInput.ColorCnt = 1;
+ dInputSetColorClkInput();
+
+ }
+ break;
+ case 1:
+ {
+ VarsInput.ColorCnt = 2;
+ }
+ break;
+ case 2:
+ {
+ VarsInput.ColorCnt = 0;
+ dInputGetAllColors(IOMapInput.Colors, VarsInput.ColorStatus);
+ }
+ break;
+ default:
+ {
+ VarsInput.ColorCnt = 0;
+ }
+ break;
+ }
+ }
+
for (Tmp = 0; Tmp < NO_OF_INPUTS; Tmp++)
{
@@ -139,38 +235,31 @@ void cInputCtrl(void)
VarsInput.InputDebounce[Tmp] = 0;
VarsInput.LastAngle[Tmp] = 0;
VarsInput.SampleCnt[Tmp] = 0;
+ VarsInput.ColorStatus &= ~(0x01<<Tmp);
+ memset(&(VarsInput.VarsColor[Tmp]),0 ,sizeof(VarsInput.VarsColor[Tmp]));
- /* 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;
+ /* Setup the pins for the new sensortype */
+ cInputSetupType(Tmp, &(IOMapInput.Inputs[Tmp].SensorType), VarsInput.OldSensorType[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 */
+ /* A type change has been carried out earlier - waiting for valid data */
+ /* The color sensor requires special startup sequence with communication */
+ if (((IOMapInput.Inputs[Tmp].SensorType) == COLORFULL) ||
+ ((IOMapInput.Inputs[Tmp].SensorType) == COLORRED) ||
+ ((IOMapInput.Inputs[Tmp].SensorType) == COLORGREEN)||
+ ((IOMapInput.Inputs[Tmp].SensorType) == COLORBLUE) ||
+ ((IOMapInput.Inputs[Tmp].SensorType) == COLOREXIT) ||
+ ((IOMapInput.Inputs[Tmp].SensorType) == COLORNONE))
+ {
+ cInputCalcSensorValues(Tmp);
+ }
+
(VarsInput.InvalidTimer[Tmp])--;
if (0 == VarsInput.InvalidTimer[Tmp])
{
@@ -190,37 +279,533 @@ void cInputCtrl(void)
if (!(INVALID_DATA & (IOMapInput.Inputs[Tmp].InvalidData)))
{
- cInputCalcSensor(Tmp);
+ cInputCalcSensorValues(Tmp);
}
}
}
-void cInputCalcSensor(UBYTE Tmp)
+void cInputCalcSensorValues(UBYTE No)
{
- UWORD InputRaw;
- /* Get the RCX hardware compensated AD values put values in */
- /* InputRaw array */
- dInputGetRawAd(&InputRaw, Tmp);
- IOMapInput.Inputs[Tmp].ADRaw = InputRaw;
+ switch(IOMapInput.Inputs[No].SensorType)
+ {
+ case SWITCH:
+ {
+ UWORD InputVal;
+
+ dInputGetRawAd(&InputVal, No);
+ IOMapInput.Inputs[No].ADRaw = InputVal;
+ cInputCalcSensorValue( InputVal,
+ &(IOMapInput.Inputs[No].SensorRaw),
+ &(IOMapInput.Inputs[No].SensorValue),
+ &(IOMapInput.Inputs[No].SensorBoolean),
+ &(VarsInput.InputDebounce[No]),
+ &(VarsInput.SampleCnt[No]),
+ &(VarsInput.LastAngle[No]),
+ &(VarsInput.EdgeCnt[No]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
+ }
+ break;
+
+ case TEMPERATURE:
+ {
+ UWORD InputVal;
+
+ dInputGetRawAd(&InputVal, No);
+ IOMapInput.Inputs[No].ADRaw = InputVal;
+ if (InputVal < 290)
+ {
+ InputVal = 290;
+ }
+ else
+ {
+ if (InputVal > 928)
+ {
+ InputVal = 928;
+ }
+ }
+ InputVal = TempConvTable[(InputVal) - 197];
+ InputVal = InputVal + 200;
+ InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900);
+ cInputCalcSensorValue( InputVal,
+ &(IOMapInput.Inputs[No].SensorRaw),
+ &(IOMapInput.Inputs[No].SensorValue),
+ &(IOMapInput.Inputs[No].SensorBoolean),
+ &(VarsInput.InputDebounce[No]),
+ &(VarsInput.SampleCnt[No]),
+ &(VarsInput.LastAngle[No]),
+ &(VarsInput.EdgeCnt[No]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
+ }
+ break;
+
+ case REFLECTION:
+ {
+ UWORD InputVal;
+
+ dInputGetRawAd(&InputVal, No);
+ IOMapInput.Inputs[No].ADRaw = InputVal;
+ cInputCalcFullScale(&InputVal, REFLECTIONSENSORMIN, REFLECTIONSENSORPCTDYN, TRUE);
+ cInputCalcSensorValue( InputVal,
+ &(IOMapInput.Inputs[No].SensorRaw),
+ &(IOMapInput.Inputs[No].SensorValue),
+ &(IOMapInput.Inputs[No].SensorBoolean),
+ &(VarsInput.InputDebounce[No]),
+ &(VarsInput.SampleCnt[No]),
+ &(VarsInput.LastAngle[No]),
+ &(VarsInput.EdgeCnt[No]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
+ }
+ break;
+
+ case ANGLE:
+ {
+ UWORD InputVal;
+
+ dInputGetRawAd(&InputVal, No);
+ IOMapInput.Inputs[No].ADRaw = InputVal;
+ cInputCalcSensorValue( InputVal,
+ &(IOMapInput.Inputs[No].SensorRaw),
+ &(IOMapInput.Inputs[No].SensorValue),
+ &(IOMapInput.Inputs[No].SensorBoolean),
+ &(VarsInput.InputDebounce[No]),
+ &(VarsInput.SampleCnt[No]),
+ &(VarsInput.LastAngle[No]),
+ &(VarsInput.EdgeCnt[No]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
+ }
+ break;
+
+ /* Dual case intended */
+ case LIGHT_ACTIVE:
+ case LIGHT_INACTIVE:
+ {
+ UWORD InputVal;
+
+ dInputGetRawAd(&InputVal, No);
+ IOMapInput.Inputs[No].ADRaw = InputVal;
+ cInputCalcFullScale(&InputVal, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE);
+ cInputCalcSensorValue( InputVal,
+ &(IOMapInput.Inputs[No].SensorRaw),
+ &(IOMapInput.Inputs[No].SensorValue),
+ &(IOMapInput.Inputs[No].SensorBoolean),
+ &(VarsInput.InputDebounce[No]),
+ &(VarsInput.SampleCnt[No]),
+ &(VarsInput.LastAngle[No]),
+ &(VarsInput.EdgeCnt[No]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
- /* Calculate the sensor hardware compensated AD values and put then */
- /* in IOMapInput.Inputs[Tmp].SensorRaw */
- cInputCalcSensorRaw(&InputRaw, IOMapInput.Inputs[Tmp].SensorType, Tmp);
+ }
+ break;
- /* 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);
+ /* Dual case intended */
+ case SOUND_DB:
+ case SOUND_DBA:
+ {
+ UWORD InputVal;
+
+ dInputGetRawAd(&InputVal, No);
+ IOMapInput.Inputs[No].ADRaw = InputVal;
+ cInputCalcFullScale(&InputVal, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE);
+ cInputCalcSensorValue( InputVal,
+ &(IOMapInput.Inputs[No].SensorRaw),
+ &(IOMapInput.Inputs[No].SensorValue),
+ &(IOMapInput.Inputs[No].SensorBoolean),
+ &(VarsInput.InputDebounce[No]),
+ &(VarsInput.SampleCnt[No]),
+ &(VarsInput.LastAngle[No]),
+ &(VarsInput.EdgeCnt[No]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
-}
+ }
+ break;
+
+ case CUSTOM:
+ {
+ UWORD InputVal;
+
+ /* Setup and read digital IO */
+ cInputSetupCustomSensor(No);
+ dInputRead0(No, &(IOMapInput.Inputs[No].DigiPinsIn));
+ dInputRead1(No, &(IOMapInput.Inputs[No].DigiPinsIn));
+
+ dInputGetRawAd(&InputVal, No);
+ IOMapInput.Inputs[No].ADRaw = InputVal;
+ cInputCalcFullScale(&InputVal, IOMapInput.Inputs[No].CustomZeroOffset, IOMapInput.Inputs[No].CustomPctFullScale, FALSE);
+ cInputCalcSensorValue( InputVal,
+ &(IOMapInput.Inputs[No].SensorRaw),
+ &(IOMapInput.Inputs[No].SensorValue),
+ &(IOMapInput.Inputs[No].SensorBoolean),
+ &(VarsInput.InputDebounce[No]),
+ &(VarsInput.SampleCnt[No]),
+ &(VarsInput.LastAngle[No]),
+ &(VarsInput.EdgeCnt[No]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
+
+ }
+ break;
+
+ /* Tripple case intended */
+ case LOWSPEED:
+ case LOWSPEED_9V:
+ case HIGHSPEED:
+ {
+ }
+ break;
+
+ /* Four cases intended */
+ case COLORRED:
+ case COLORGREEN:
+ case COLORBLUE:
+ case COLORNONE:
+ {
+
+ UWORD InputVal;
+ switch (IOMapInput.Colors[No].CalibrationState)
+ {
+ case SENSOROFF:
+ {
+ /* Make sure that sensor data are invalid while unplugged*/
+ VarsInput.InvalidTimer[No] = INVALID_RELOAD_COLOR;
+ IOMapInput.Inputs[No].InvalidData = INVALID_DATA;
+ /* Check if sensor has been attached */
+ if (dInputCheckColorStatus(No))
+ {
-void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
+ /* Sensor has been attached now get cal data */
+ VarsInput.VarsColor[No].ColorInitState = 0;
+ (IOMapInput.Colors[No].CalibrationState) = SENSORCAL;
+ }
+ }
+ break;
+ case SENSORCAL:
+ {
+
+ UBYTE Status;
+ if (FALSE == cInputInitColorSensor(No, &Status))
+ {
+
+ /* Color sensor has been removed during calibration */
+ (IOMapInput.Colors[No].CalibrationState) = SENSOROFF;
+ }
+
+ if (TRUE == Status)
+ {
+
+ /* Use clock to detect errors */
+ dInputSetDirInDigi0(No);
+ (IOMapInput.Colors[No].CalibrationState) = 0;
+ }
+ }
+ break;
+ default:
+ {
+ if (dInputGetColor(No, &(IOMapInput.Inputs[No].ADRaw)))
+ {
+ InputVal = IOMapInput.Inputs[No].ADRaw;
+ cInputCalcFullScale(&InputVal, COLORSENSORBGMIN, COLORSENSORBGPCTDYN, FALSE);
+ cInputCalcSensorValue(InputVal,
+ &(IOMapInput.Inputs[No].SensorRaw),
+ &(IOMapInput.Inputs[No].SensorValue),
+ &(IOMapInput.Inputs[No].SensorBoolean),
+ &(VarsInput.InputDebounce[No]),
+ &(VarsInput.SampleCnt[No]),
+ &(VarsInput.LastAngle[No]),
+ &(VarsInput.EdgeCnt[No]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
+ }
+ else
+ {
+ IOMapInput.Colors[No].CalibrationState = SENSOROFF;
+ }
+ }
+ break;
+ }
+ }
+ break;
+ case COLORFULL:
+ {
+ switch (IOMapInput.Colors[No].CalibrationState)
+ {
+ case SENSOROFF:
+ {
+
+ /* Make sure that sensor data are invalid while unplugged */
+ VarsInput.InvalidTimer[No] = INVALID_RELOAD_COLOR;
+ IOMapInput.Inputs[No].InvalidData = INVALID_DATA;
+
+ /* Check if sensor has been attached */
+ if (dInputCheckColorStatus(No))
+ {
+
+ /* Sensor has been attached now get cal data */
+ VarsInput.VarsColor[No].ColorInitState = 0;
+ (IOMapInput.Colors[No].CalibrationState) = SENSORCAL;
+ }
+ }
+ break;
+ case SENSORCAL:
+ {
+ UBYTE Status;
+
+ if (FALSE == cInputInitColorSensor(No, &Status))
+ {
+
+ /* Color sensor has been removed during calibration */
+ (IOMapInput.Colors[No].CalibrationState) = SENSOROFF;
+ VarsInput.ColorStatus &= ~(0x01<<No);
+ }
+
+ if (TRUE == Status)
+ {
+
+ /* Initialization finished with success recalc the values*/
+ (IOMapInput.Colors[No].CalibrationState) = 0;
+
+ /* Calculate Calibration factor */
+ VarsInput.ColorStatus |= (0x01<<No);
+
+ }
+ }
+ break;
+ default:
+ {
+
+ /* calculate only when new ad values are ready */
+ if (0 == VarsInput.ColorCnt)
+ {
+
+ UWORD NewSensorVals[NO_OF_COLORS];
+ UBYTE ColorCount;
+
+ COLORSTRUCT *pC;
+
+ pC = &(IOMapInput.Colors[No]);
+
+ /* Check if sensor is deteched */
+ if (dInputCheckColorStatus(No))
+ {
+
+ /* Calibrate the raw ad values returns the SensorRaw */
+ cInputCalibrateColor(pC, NewSensorVals);
+
+ for(ColorCount = 0; ColorCount < BLANK; ColorCount++)
+ {
+
+ /* Calculate color sensor values */
+ cInputCalcSensorValue(NewSensorVals[ColorCount],
+ &(IOMapInput.Colors[No].SensorRaw[ColorCount]),
+ &(IOMapInput.Colors[No].SensorValue[ColorCount]),
+ &(IOMapInput.Colors[No].Boolean[ColorCount]),
+ &(VarsInput.VarsColor[No].ColorInputDebounce[ColorCount]),
+ &(VarsInput.VarsColor[No].ColorSampleCnt[ColorCount]),
+ &(VarsInput.VarsColor[No].ColorLastAngle[ColorCount]),
+ &(VarsInput.VarsColor[No].ColorEdgeCnt[ColorCount]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
+ }
+
+ /* Calculate background sensor values */
+ cInputCalcSensorValue(NewSensorVals[BLANK],
+ &(IOMapInput.Colors[No].SensorRaw[BLANK]),
+ &(IOMapInput.Colors[No].SensorValue[BLANK]),
+ &(IOMapInput.Colors[No].Boolean[BLANK]),
+ &(VarsInput.VarsColor[No].ColorInputDebounce[BLANK]),
+ &(VarsInput.VarsColor[No].ColorSampleCnt[BLANK]),
+ &(VarsInput.VarsColor[No].ColorLastAngle[BLANK]),
+ &(VarsInput.VarsColor[No].ColorEdgeCnt[BLANK]),
+ ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
+ ((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
+
+ /* Color Sensor values has been calculated - */
+ /* now calculate the color and put it in Sensor value */
+ if (((pC->SensorRaw[RED]) > (pC->SensorRaw[BLUE] )) &&
+ ((pC->SensorRaw[RED]) > (pC->SensorRaw[GREEN])))
+ {
+
+ /* If all 3 colors are less than 65 OR (Less that 110 and bg less than 40)*/
+ if (((pC->SensorRaw[RED]) < 65) ||
+ (((pC->SensorRaw[BLANK]) < 40) && ((pC->SensorRaw[RED]) < 110)))
+ {
+ IOMapInput.Inputs[No].SensorValue = BLACKCOLOR;
+ }
+ else
+ {
+ if (((((pC->SensorRaw[BLUE]) >> 2) + ((pC->SensorRaw[BLUE]) >> 3) + (pC->SensorRaw[BLUE])) < (pC->SensorRaw[GREEN])) &&
+ ((((pC->SensorRaw[GREEN]) << 1)) > (pC->SensorRaw[RED])))
+ {
+ IOMapInput.Inputs[No].SensorValue = YELLOWCOLOR;
+ }
+ else
+ {
+
+ if ((((pC->SensorRaw[GREEN]) << 1) - ((pC->SensorRaw[GREEN]) >> 2)) < (pC->SensorRaw[RED]))
+ {
+
+ IOMapInput.Inputs[No].SensorValue = REDCOLOR;
+ }
+ else
+ {
+
+ if ((((pC->SensorRaw[BLUE]) < 70) ||
+ ((pC->SensorRaw[GREEN]) < 70)) ||
+ (((pC->SensorRaw[BLANK]) < 140) && ((pC->SensorRaw[RED]) < 140)))
+ {
+ IOMapInput.Inputs[No].SensorValue = BLACKCOLOR;
+ }
+ else
+ {
+ IOMapInput.Inputs[No].SensorValue = WHITECOLOR;
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+
+ /* Red is not the dominant color */
+ if ((pC->SensorRaw[GREEN]) > (pC->SensorRaw[BLUE]))
+ {
+
+ /* Green is the dominant color */
+ /* If all 3 colors are less than 40 OR (Less that 70 and bg less than 20)*/
+ if (((pC->SensorRaw[GREEN]) < 40) ||
+ (((pC->SensorRaw[BLANK]) < 30) && ((pC->SensorRaw[GREEN]) < 70)))
+ {
+ IOMapInput.Inputs[No].SensorValue = BLACKCOLOR;
+ }
+ else
+ {
+ if ((((pC->SensorRaw[BLUE]) << 1)) < (pC->SensorRaw[RED]))
+ {
+ IOMapInput.Inputs[No].SensorValue = YELLOWCOLOR;
+ }
+ else
+ {
+ if ((((pC->SensorRaw[RED]) + ((pC->SensorRaw[RED])>>2)) < (pC->SensorRaw[GREEN])) ||
+ (((pC->SensorRaw[BLUE]) + ((pC->SensorRaw[BLUE])>>2)) < (pC->SensorRaw[GREEN])))
+ {
+ IOMapInput.Inputs[No].SensorValue = GREENCOLOR;
+ }
+ else
+ {
+ if ((((pC->SensorRaw[RED]) < 70) ||
+ ((pC->SensorRaw[BLUE]) < 70)) ||
+ (((pC->SensorRaw[BLANK]) < 140) && ((pC->SensorRaw[GREEN]) < 140)))
+ {
+ IOMapInput.Inputs[No].SensorValue = BLACKCOLOR;
+ }
+ else
+ {
+ IOMapInput.Inputs[No].SensorValue = WHITECOLOR;
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+
+ /* Blue is the most dominant color */
+ /* Colors can be blue, white or black */
+ /* If all 3 colors are less than 48 OR (Less that 85 and bg less than 25)*/
+ if (((pC->SensorRaw[BLUE]) < 48) ||
+ (((pC->SensorRaw[BLANK]) < 25) && ((pC->SensorRaw[BLUE]) < 85)))
+ {
+ IOMapInput.Inputs[No].SensorValue = BLACKCOLOR;
+ }
+ else
+ {
+ if ((((((pC->SensorRaw[RED]) * 48) >> 5) < (pC->SensorRaw[BLUE])) &&
+ ((((pC->SensorRaw[GREEN]) * 48) >> 5) < (pC->SensorRaw[BLUE])))
+ ||
+ (((((pC->SensorRaw[RED]) * 58) >> 5) < (pC->SensorRaw[BLUE])) ||
+ ((((pC->SensorRaw[GREEN]) * 58) >> 5) < (pC->SensorRaw[BLUE]))))
+ {
+ IOMapInput.Inputs[No].SensorValue = BLUECOLOR;
+ }
+ else
+ {
+
+ /* Color is white or Black */
+ if ((((pC->SensorRaw[RED]) < 60) ||
+ ((pC->SensorRaw[GREEN]) < 60)) ||
+ (((pC->SensorRaw[BLANK]) < 110) && ((pC->SensorRaw[BLUE]) < 120)))
+ {
+ IOMapInput.Inputs[No].SensorValue = BLACKCOLOR;
+ }
+ else
+ {
+ if ((((pC->SensorRaw[RED]) + ((pC->SensorRaw[RED]) >> 3)) < (pC->SensorRaw[BLUE])) ||
+ (((pC->SensorRaw[GREEN]) + ((pC->SensorRaw[GREEN]) >> 3)) < (pC->SensorRaw[BLUE])))
+ {
+ IOMapInput.Inputs[No].SensorValue = BLUECOLOR;
+ }
+ else
+ {
+ IOMapInput.Inputs[No].SensorValue = WHITECOLOR;
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ IOMapInput.Colors[No].CalibrationState = SENSOROFF;
+ VarsInput.ColorStatus &= ~(0x01<<No);
+ }
+ }
+ break;
+ }
+ }
+ }
+ break;
+ case COLOREXIT:
+ {
+ UBYTE Status;
+
+ VarsInput.ColorStatus &= ~(0x01<<No);
+ if (FALSE == cInputInitColorSensor(No, &Status))
+ {
+ IOMapInput.Inputs[No].SensorType = NO_SENSOR;
+ }
+
+ if (TRUE == Status)
+ {
+
+ /* Initialization finished with success recalc the values*/
+ (IOMapInput.Colors[No].CalibrationState) = 0;
+ IOMapInput.Inputs[No].SensorType = NO_SENSOR;
+ VarsInput.OldSensorType[No] = NO_SENSOR;
+ }
+ }
+ break;
+ default:
+ {
+ }
+ break;
+ }
+}
+
+
+void cInputCalcSensorValue(UWORD NewSensorRaw, UWORD *pOldSensorRaw, SWORD *pSensorValue,
+ UBYTE *pBoolean, UBYTE *pDebounce, UBYTE *pSampleCnt,
+ UBYTE *LastAngle, UBYTE *pEdgeCnt, UBYTE Slope,
+ UBYTE Mode)
{
SWORD Delta;
UBYTE PresentBoolean;
@@ -230,13 +815,13 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
{
/* This is absolute measure method */
- if (*pRaw > THRESHOLD_FALSE)
+ if (NewSensorRaw > THRESHOLD_FALSE)
{
PresentBoolean = FALSE;
}
else
{
- if (*pRaw < THRESHOLD_TRUE)
+ if (NewSensorRaw < THRESHOLD_TRUE)
{
PresentBoolean = TRUE;
}
@@ -246,19 +831,19 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
{
/* This is dynamic measure method */
- if (*pRaw > (ACTUAL_AD_RES - Slope))
+ if (NewSensorRaw > (ACTUAL_AD_RES - Slope))
{
PresentBoolean = FALSE;
}
else
{
- if (*pRaw < Slope)
+ if (NewSensorRaw < Slope)
{
PresentBoolean = TRUE;
}
else
{
- Delta = IOMapInput.Inputs[Tmp].SensorRaw - *pRaw;
+ Delta = *pOldSensorRaw - NewSensorRaw;
if (Delta < 0)
{
if (-Delta > Slope)
@@ -276,35 +861,35 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
}
}
}
- IOMapInput.Inputs[Tmp].SensorRaw = *pRaw;
+ *pOldSensorRaw = NewSensorRaw;
switch(Mode)
{
case RAWMODE:
{
- IOMapInput.Inputs[Tmp].SensorValue = *pRaw;
+ *pSensorValue = NewSensorRaw;
}
break;
case BOOLEANMODE:
{
- IOMapInput.Inputs[Tmp].SensorValue = PresentBoolean;
+ *pSensorValue = PresentBoolean;
}
break;
case TRANSITIONCNTMODE:
{
- if (VarsInput.InputDebounce[Tmp] > 0)
+ if ((*pDebounce) > 0)
{
- VarsInput.InputDebounce[Tmp]--;
+ (*pDebounce)--;
}
else
{
- if (IOMapInput.Inputs[Tmp].SensorBoolean != PresentBoolean)
+ if (*pBoolean != PresentBoolean)
{
- VarsInput.InputDebounce[Tmp] = DEBOUNCERELOAD;
- (IOMapInput.Inputs[Tmp].SensorValue)++;
+ (*pDebounce) = DEBOUNCERELOAD;
+ (*pSensorValue)++;
}
}
}
@@ -312,22 +897,22 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
case PERIODCOUNTERMODE:
{
- if (VarsInput.InputDebounce[Tmp] > 0)
+ if ((*pDebounce) > 0)
{
- VarsInput.InputDebounce[Tmp]--;
+ (*pDebounce)--;
}
else
{
- if (IOMapInput.Inputs[Tmp].SensorBoolean != PresentBoolean)
+ if (*pBoolean != PresentBoolean)
{
- VarsInput.InputDebounce[Tmp] = DEBOUNCERELOAD;
- IOMapInput.Inputs[Tmp].SensorBoolean = PresentBoolean;
- if (++VarsInput.EdgeCnt[Tmp] > 1)
+ (*pDebounce) = DEBOUNCERELOAD;
+ *pBoolean = PresentBoolean;
+ if (++(*pEdgeCnt) > 1)
{
if (PresentBoolean == 0)
{
- VarsInput.EdgeCnt[Tmp] = 0;
- (IOMapInput.Inputs[Tmp].SensorValue)++;
+ (*pEdgeCnt) = 0;
+ (*pSensorValue)++;
}
}
}
@@ -339,7 +924,7 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
{
/* Output is 0-100 pct */
- IOMapInput.Inputs[Tmp].SensorValue = ((*pRaw) * 100)/SENSOR_RESOLUTION;
+ *pSensorValue = ((NewSensorRaw) * 100)/SENSOR_RESOLUTION;
}
break;
@@ -347,8 +932,8 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
{
/* 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;
+ *pSensorValue = (((ULONG)(NewSensorRaw) * 900L)/SENSOR_RESOLUTION) - 200;
+ *pSensorValue = ((180L * (ULONG)(*pSensorValue))/100L) + 320;
}
break;
@@ -356,27 +941,27 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
{
/* Celsius mode goes from -20 to 70 degrees */
- IOMapInput.Inputs[Tmp].SensorValue = (((ULONG)(*pRaw) * 900L)/SENSOR_RESOLUTION) - 200;
+ *pSensorValue = (((ULONG)(NewSensorRaw * 900L)/SENSOR_RESOLUTION) - 200);
}
break;
case ANGLESTEPSMODE:
{
- IOMapInput.Inputs[Tmp].SensorBoolean = PresentBoolean;
+ *pBoolean = PresentBoolean;
- if (*pRaw < ANGLELIMITA)
+ if (NewSensorRaw < ANGLELIMITA)
{
Sample = 0;
}
else
{
- if (*pRaw < ANGLELIMITB)
+ if (NewSensorRaw < ANGLELIMITB)
{
Sample = 1;
}
else
{
- if (*pRaw < ANGLELIMITC)
+ if (NewSensorRaw < ANGLELIMITC)
{
Sample = 2;
}
@@ -387,37 +972,37 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
}
}
- switch (VarsInput.LastAngle[Tmp])
+ switch (*LastAngle)
{
case 0 :
{
if (Sample == 1)
{
- if (VarsInput.SampleCnt[Tmp] >= ROT_SLOW_SPEED )
+ if ((*pSampleCnt) >= ROT_SLOW_SPEED )
{
- if (++(VarsInput.SampleCnt[Tmp]) >= (ROT_SLOW_SPEED + ROT_OV_SAMPLING))
+ if (++(*pSampleCnt) >= (ROT_SLOW_SPEED + ROT_OV_SAMPLING))
{
- (IOMapInput.Inputs[Tmp].SensorValue)++;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)++;
+ (*LastAngle) = Sample;
}
}
else
{
- (IOMapInput.Inputs[Tmp].SensorValue)++;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)++;
+ (*LastAngle) = Sample;
}
}
if (Sample == 2)
{
- (IOMapInput.Inputs[Tmp].SensorValue)--;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)--;
+ (*LastAngle) = Sample;
}
if (Sample == 0)
{
- if (VarsInput.SampleCnt[Tmp] < ROT_SLOW_SPEED)
+ if ((*pSampleCnt) < ROT_SLOW_SPEED)
{
- (VarsInput.SampleCnt[Tmp])++;
+ (*pSampleCnt)++;
}
}
}
@@ -426,61 +1011,61 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
{
if (Sample == 3)
{
- (IOMapInput.Inputs[Tmp].SensorValue)++;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)++;
+ (*LastAngle) = Sample;
}
if (Sample == 0)
{
- (IOMapInput.Inputs[Tmp].SensorValue)--;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)--;
+ (*LastAngle) = Sample;
}
- VarsInput.SampleCnt[Tmp] = 0;
+ (*pSampleCnt) = 0;
}
break;
case 2 :
{
if (Sample == 0)
{
- (IOMapInput.Inputs[Tmp].SensorValue)++;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)++;
+ (*LastAngle) = Sample;
}
if (Sample == 3)
{
- (IOMapInput.Inputs[Tmp].SensorValue)--;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)--;
+ (*LastAngle) = Sample;
}
- VarsInput.SampleCnt[Tmp] = 0;
+ (*pSampleCnt) = 0;
}
break;
case 3 :
{
if (Sample == 2)
{
- if (VarsInput.SampleCnt[Tmp] >= ROT_SLOW_SPEED)
+ if ((*pSampleCnt) >= ROT_SLOW_SPEED)
{
- if (++(VarsInput.SampleCnt[Tmp]) >= (ROT_SLOW_SPEED + ROT_OV_SAMPLING))
+ if (++(*pSampleCnt) >= (ROT_SLOW_SPEED + ROT_OV_SAMPLING))
{
- (IOMapInput.Inputs[Tmp].SensorValue)++;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)++;
+ (*LastAngle) = Sample;
}
}
else
{
- (IOMapInput.Inputs[Tmp].SensorValue)++;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)++;
+ (*LastAngle) = Sample;
}
}
if (Sample == 1)
{
- (IOMapInput.Inputs[Tmp].SensorValue)--;
- VarsInput.LastAngle[Tmp] = Sample;
+ (*pSensorValue)--;
+ (*LastAngle) = Sample;
}
if (Sample == 3)
{
- if (VarsInput.SampleCnt[Tmp] < ROT_SLOW_SPEED)
+ if ((*pSampleCnt) < ROT_SLOW_SPEED)
{
- (VarsInput.SampleCnt[Tmp])++;
+ (*pSampleCnt)++;
}
}
}
@@ -489,149 +1074,202 @@ void cInputCalcSensorValue(UWORD *pRaw, UBYTE Slope, UBYTE Mode, UBYTE Tmp)
}
}
- IOMapInput.Inputs[Tmp].SensorBoolean = PresentBoolean;
+ *pBoolean = PresentBoolean;
}
-
-const SWORD TempConvTable[] =
+void cInputCalcFullScale(UWORD *pRawVal, UWORD ZeroPointOffset, UBYTE PctFullScale, UBYTE InvStatus)
{
- 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
-};
+ 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 cInputCalcSensorRaw(UWORD *pRaw, UBYTE Type, UBYTE No)
+void cInputSetupType(UBYTE Port, UBYTE *pType, UBYTE OldType)
{
- switch (Type)
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_NORMAL;
+
+ /* If old type is color sensor in color lamp mode then turn off leds */
+ switch (OldType)
{
- case SWITCH:
+ case COLORRED:
+ case COLORGREEN:
+ case COLORBLUE:
+ case COLORFULL:
+ case COLOREXIT:
{
+ if (NO_SENSOR == *pType)
+ {
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR;
+ *pType = COLOREXIT;
+ }
}
break;
+ }
+ switch(*pType)
+ {
+ case NO_SENSOR:
+ case SWITCH:
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);
+ dInputSetInactive(Port);
+ dInputSetDirInDigi0(Port);
+ dInputSetDirInDigi1(Port);
}
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);
+ dInputSetActive(Port);
+ dInputClearDigi0(Port);
+ dInputClearDigi1(Port);
}
break;
+
case ANGLE:
{
+ dInputSetActive(Port);
+ dInputClearDigi0(Port);
+ dInputClearDigi1(Port);
}
break;
+
case LIGHT_ACTIVE:
{
- cInputCalcFullScale(pRaw, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE);
+ dInputSetInactive(Port);
+ dInputSetDigi0(Port);
+ dInputClearDigi1(Port);
}
break;
+
case LIGHT_INACTIVE:
{
- cInputCalcFullScale(pRaw, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE);
+ dInputSetInactive(Port);
+ dInputClearDigi0(Port);
+ dInputClearDigi1(Port);
}
break;
+
case SOUND_DB:
{
- cInputCalcFullScale(pRaw, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE);
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_SOUND;
+ dInputSetInactive(Port);
+ dInputSetDigi0(Port);
+ dInputClearDigi1(Port);
}
break;
+
case SOUND_DBA:
{
- cInputCalcFullScale(pRaw, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE);
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_SOUND;
+ dInputSetInactive(Port);
+ dInputClearDigi0(Port);
+ dInputSetDigi1(Port);
}
break;
+
+ case CUSTOM:
+ {
+ cInputSetupCustomSensor(Port);
+ }
+ break;
+
case LOWSPEED:
{
- /* Intended empty Low Speed module takes over */
+ dInputSetInactive(Port);
+ dInputSetDigi0(Port);
+ dInputSetDigi1(Port);
+ }
+ break;
+
+ case LOWSPEED_9V:
+ {
+ dInputSet9v(Port);
+ dInputSetDigi0(Port);
+ dInputSetDigi1(Port);
}
break;
+
case HIGHSPEED:
{
+ dInputSetInactive(Port);
+ dInputSetDirInDigi0(Port);
+ dInputSetDirInDigi1(Port);
}
break;
- case CUSTOM:
+
+ case COLORFULL:
{
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR;
+ dInputSetInactive(Port);
+ dInputSetDigi0(Port);
+ dInputSetDirInDigi1(Port);
+ IOMapInput.Colors[Port].CalibrationState = SENSORCAL;
+ VarsInput.VarsColor[Port].ColorInitState = 0;
- /* 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:
+
+ case COLORRED:
+ {
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR;
+ dInputSetInactive(Port);
+ dInputSetDigi0(Port);
+ dInputSetDirInDigi1(Port);
+ IOMapInput.Colors[Port].CalibrationState = SENSORCAL;
+ VarsInput.VarsColor[Port].ColorInitState = 0;
+ }
+ break;
+
+ case COLORGREEN:
{
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR;
+ dInputSetInactive(Port);
+ dInputSetDigi0(Port);
+ dInputSetDirInDigi1(Port);
+ IOMapInput.Colors[Port].CalibrationState = SENSORCAL;
+ VarsInput.VarsColor[Port].ColorInitState = 0;
}
break;
+
+ case COLORBLUE:
+ {
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR;
+ dInputSetInactive(Port);
+ dInputSetDigi0(Port);
+ dInputSetDirInDigi1(Port);
+ IOMapInput.Colors[Port].CalibrationState = SENSORCAL;
+ VarsInput.VarsColor[Port].ColorInitState = 0;
+ }
+ break;
+
+ case COLORNONE:
+ {
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR;
+ dInputSetInactive(Port);
+ dInputSetDigi0(Port);
+ dInputSetDirInDigi1(Port);
+ IOMapInput.Colors[Port].CalibrationState = SENSORCAL;
+ VarsInput.VarsColor[Port].ColorInitState = 0;
+ }
+ break;
+
default:
{
}
@@ -639,139 +1277,247 @@ void cInputCalcSensorRaw(UWORD *pRaw, UBYTE Type, UBYTE No)
}
}
-void cInputCalcFullScale(UWORD *pRawVal, UWORD ZeroPointOffset, UBYTE PctFullScale, UBYTE InvStatus)
+void cInputSetupCustomSensor(UBYTE Port)
{
- if (*pRawVal >= ZeroPointOffset)
+ if ((IOMapInput.Inputs[Port].DigiPinsDir) & 0x01)
{
- *pRawVal -= ZeroPointOffset;
+ if ((IOMapInput.Inputs[Port].DigiPinsOut) & 0x01)
+ {
+ dInputSetDigi0(Port);
+ }
+ else
+ {
+ dInputClearDigi0(Port);
+ }
+ }
+ if ((IOMapInput.Inputs[Port].DigiPinsDir) & 0x02)
+ {
+ if ((IOMapInput.Inputs[Port].DigiPinsOut) & 0x02)
+ {
+ dInputSetDigi1(Port);
+ }
+ else
+ {
+ dInputClearDigi1(Port);
+ }
}
else
{
- *pRawVal = 0;
+ dInputSetDirInDigi1(Port);
}
- *pRawVal = (*pRawVal * 100)/PctFullScale;
- if (*pRawVal > SENSOR_RESOLUTION)
+ if (CUSTOMACTIVE == (IOMapInput.Inputs[Port].CustomActiveStatus))
{
- *pRawVal = SENSOR_RESOLUTION;
+ dInputSetActive(Port);
}
- if (TRUE == InvStatus)
+ else
{
- *pRawVal = SENSOR_RESOLUTION - *pRawVal;
+ if (CUSTOM9V == (IOMapInput.Inputs[Port].CustomActiveStatus))
+ {
+ dInputSet9v(Port);
+ }
+ else
+ {
+ dInputSetInactive(Port);
+ }
}
}
-void cInputSetupType(UBYTE Port)
-{
- UBYTE Setup;
- Setup = (ActiveList[IOMapInput.Inputs[Port].SensorType]);
+UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus)
+{
- if (CUSTOM_SETUP & Setup)
+ *pInitStatus = FALSE;
+ switch(VarsInput.VarsColor[Port].ColorInitState)
{
- cInputSetupCustomSensor(Port);
- }
- else
- {
- if (NO_POWER & Setup)
+ case 0:
+ {
+ dInputSetDigi0(Port);
+ dInputSetDigi1(Port);
+ VarsInput.VarsColor[Port].ColorInitState++;
+ }
+ break;
+ case 1:
+ {
+ dInputClearDigi0(Port);
+ VarsInput.VarsColor[Port].ColorInitState++;
+ }
+ break;
+
+ case 2:
+ {
+ dInputSetDigi0(Port);
+ VarsInput.VarsColor[Port].ColorInitState++;
+ }
+ break;
+ case 3:
{
- /* Setup is not used - set pins in unconfigured state */
- dInputSetInactive(Port);
- dInputSetDirInDigi0(Port);
- dInputSetDirInDigi1(Port);
+ dInputClearDigi0(Port);
+
+ /* Clear clock for 100mS - use pit timer*/
+ dInputClearColor100msTimer(Port);
+ VarsInput.VarsColor[Port].ColorInitState++;
}
- else
+ break;
+ case 4:
{
- if (ACTIVE & Setup)
+
+ /* Wait 100mS */
+ if (dInputChkColor100msTimer(Port))
{
- dInputSetActive(Port);
+ VarsInput.VarsColor[Port].ColorInitState += 1;
}
- else
+ }
+ break;
+ case 5:
+ {
+ UBYTE TmpType;
+
+ if (COLOREXIT == IOMapInput.Inputs[Port].SensorType)
{
- if(ALWAYS_ACTIVE & Setup)
- {
- dInputSet9v(Port);
- }
- else
- {
- dInputSetInactive(Port);
- }
+ TmpType = COLORNONE;
}
- if (DIGI_0_HIGH & Setup)
+ else
{
- dInputSetDigi0(Port);
- dInputSetDirOutDigi0(Port);
+ TmpType = IOMapInput.Inputs[Port].SensorType;
}
- else
+ dInputColorTx(Port, TmpType);
+
+ /* Be ready to receive data from sensor */
+ dInputSetDirInDigi1(Port);
+ VarsInput.VarsColor[Port].ReadCnt = 0;
+ VarsInput.VarsColor[Port].ColorInitState++;
+ }
+ break;
+ case 6:
+ {
+ UBYTE Data;
+ UBYTE DataCnt;
+ UBYTE *pData;
+
+ DataCnt = (VarsInput.VarsColor[Port].ReadCnt);
+ pData = (UBYTE*)(IOMapInput.Colors[Port].Calibration);
+
+ /* Read first byte of cal data */
+ dInputReadCal(Port, &Data);
+
+ pData[DataCnt] = Data;
+
+ /* If all bytes has been read - then continue to next step */
+ if (++(VarsInput.VarsColor[Port].ReadCnt) >= ((sizeof(IOMapInput.Colors[Port].Calibration) + sizeof(IOMapInput.Colors[Port].CalLimits))))
{
- dInputClearDigi0(Port);
- dInputSetDirOutDigi0(Port);
+ VarsInput.VarsColor[Port].ColorInitState++;
}
+ }
+ break;
+ case 7:
+ {
+
+ /* Check CRC then continue or restart if false */
+ UWORD Crc, CrcCheck;
+ UBYTE Cnt;
+ UBYTE Data;
+ UBYTE *pData;
+
+ dInputReadCal(Port, &Data);
+ Crc = (UWORD)(Data) << 8;
+ dInputReadCal(Port, &Data);
+ Crc += (UWORD)Data;
+ CrcCheck = 0x5AA5;
+ pData = (UBYTE*)(IOMapInput.Colors[Port].Calibration);
+ for (Cnt = 0; Cnt < (sizeof(IOMapInput.Colors[Port].Calibration) + sizeof(IOMapInput.Colors[Port].CalLimits)); Cnt++)
+ {
+ UWORD i,j;
+ UBYTE c;
+ c = pData[Cnt];
+ for(i = 0; i != 8; c >>= 1, i++)
+ {
+ j = (c^CrcCheck) & 1;
+ CrcCheck >>= 1;
+
+ if(j)
+ {
+ CrcCheck ^= 0xA001;
+ }
+ }
- if (DIGI_1_HIGH & Setup)
+ }
+ if ((CrcCheck != Crc))
{
- dInputSetDigi1(Port);
- dInputSetDirOutDigi1(Port);
+
+ /* incorrect!!! try again */
+ VarsInput.VarsColor[Port].ColorInitState = 0;
+ VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR;
}
else
{
- dInputClearDigi1(Port);
- dInputSetDirOutDigi1(Port);
+
+ /* Correct crc sum -> calculate the calibration values then exit */
+ VarsInput.VarsColor[Port].ColorInitState = 0;
+
+ /* Sensor is almost ready - needs a little time to make first measurements */
+ VarsInput.InvalidTimer[Port] = 10;
+ *pInitStatus = TRUE;
}
}
+ break;
+ default:
+ {
+ VarsInput.VarsColor[Port].ColorInitState = 0;
+ }
+ break;
}
+ return(dInputCheckColorStatus(Port));
}
-void cInputSetupCustomSensor(UBYTE Port)
+
+void cInputCalibrateColor(COLORSTRUCT *pC, UWORD *pNewVals)
{
- if ((IOMapInput.Inputs[Port].DigiPinsDir) & 0x01)
+ UBYTE CalRange;
+
+ if ((pC->ADRaw[BLANK]) < pC->CalLimits[1])
{
- if ((IOMapInput.Inputs[Port].DigiPinsOut) & 0x01)
- {
- dInputSetDigi0(Port);
- }
- else
- {
- dInputClearDigi0(Port);
- }
- dInputSetDirOutDigi0(Port);
+ CalRange = 2;
}
- if ((IOMapInput.Inputs[Port].DigiPinsDir) & 0x02)
+ else
{
- if ((IOMapInput.Inputs[Port].DigiPinsOut) & 0x02)
+ if ((pC->ADRaw[BLANK]) < pC->CalLimits[0])
{
- dInputSetDigi1(Port);
+ CalRange = 1;
}
else
{
- dInputClearDigi1(Port);
+ CalRange = 0;
}
- dInputSetDirOutDigi1(Port);
}
- else
+
+ pNewVals[RED] = 0;
+ if ((pC->ADRaw[RED]) > (pC->ADRaw[BLANK]))
{
- dInputSetDirInDigi1(Port);
+ pNewVals[RED] = (UWORD)(((ULONG)((pC->ADRaw[RED]) - (pC->ADRaw[BLANK])) * (pC->Calibration[CalRange][RED])) >> 16);
}
- if (CUSTOMACTIVE == (IOMapInput.Inputs[Port].CustomActiveStatus))
+ pNewVals[GREEN] = 0;
+ if ((pC->ADRaw[GREEN]) > (pC->ADRaw[BLANK]))
{
- dInputSetActive(Port);
+ pNewVals[GREEN] = (UWORD)(((ULONG)((pC->ADRaw[GREEN]) - (pC->ADRaw[BLANK])) * (pC->Calibration[CalRange][GREEN])) >> 16);
}
- else
+
+ pNewVals[BLUE] = 0;
+ if ((pC->ADRaw[BLUE]) > (pC->ADRaw[BLANK]))
{
- if (CUSTOM9V == (IOMapInput.Inputs[Port].CustomActiveStatus))
- {
- dInputSet9v(Port);
- }
- else
- {
- dInputSetInactive(Port);
- }
+ pNewVals[BLUE] = (UWORD)(((ULONG)((pC->ADRaw[BLUE]) -(pC->ADRaw[BLANK])) * (pC->Calibration[CalRange][BLUE])) >> 16);
}
+
+ pNewVals[BLANK] = (pC->ADRaw[BLANK]);
+ cInputCalcFullScale(&(pNewVals[BLANK]), COLORSENSORBGMIN, COLORSENSORBGPCTDYN, FALSE);
+ (pNewVals[BLANK]) = (UWORD)(((ULONG)(pNewVals[BLANK]) * (pC->Calibration[CalRange][BLANK])) >> 16);
}
+
void cInputExit(void)
{
dInputExit();
}
+