aboutsummaryrefslogtreecommitdiffhomepage
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.c1335
1 files changed, 0 insertions, 1335 deletions
diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c
deleted file mode 100644
index 2cb9be1..0000000
--- a/AT91SAM7S256/Source/c_input.c
+++ /dev/null
@@ -1,1335 +0,0 @@
-
-//
-// Date init 14.12.2004
-//
-// Revision date $Date:: 3/21/09 10:31a $
-//
-// Filename $Workfile:: c_input.c $
-//
-// Version $Revision:: 39 $
-//
-// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_inpu $
-//
-// Platform C
-//
-
-#include "stdconst.h"
-#include "modules.h"
-#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
-
-#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)
-
-/* 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,
- NO_POWER = 0x01,
- ACTIVE = 0x02,
- ALWAYS_ACTIVE = 0x04,
- DIGI_0_HIGH = 0x08,
- DIGI_1_HIGH = 0x10,
- DIGI_0_IN = 0x20,
- DIGI_1_IN = 0x40,
- CUSTOM_SETUP = 0x80
-};
-
-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 cInputCalcFullScale(UWORD *pRawVal, UWORD ZeroPointOffset, UBYTE PctFullScale, UBYTE InvState);
-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);
-SWORD cInputTempConv(UWORD InputVal);
-
-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++)
- {
- 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;
- }
-
- VarsInput.ColorStatus = 0;
- VarsInput.ColorCnt = 0;
-
- dInputInit();
-}
-
-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++)
- {
- UBYTE sType = IOMapInput.Inputs[Tmp].SensorType;
- UBYTE *pType = &IOMapInput.Inputs[Tmp].SensorType;
- UBYTE oldType = VarsInput.OldSensorType[Tmp];
-
- if (sType != oldType)
- {
-
- /* Clear all variables for this sensor */
- VarsInput.EdgeCnt[Tmp] = 0;
- 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]));
-
- /* Setup the pins for the new sensortype */
- cInputSetupType(Tmp, pType, oldType);
- sType = *pType;
- IOMapInput.Inputs[Tmp].InvalidData = INVALID_DATA;
- VarsInput.OldSensorType[Tmp] = sType;
- }
- else
- {
- if (VarsInput.InvalidTimer[Tmp])
- {
-
- /* A type change has been carried out earlier - waiting for valid data */
- /* The color sensor requires special startup sequence with communication */
- if ((sType == COLORFULL) || (sType == COLORRED) ||
- (sType == COLORGREEN)|| (sType == COLORBLUE) ||
- (sType == COLOREXIT) || (sType == COLORNONE))
- {
- cInputCalcSensorValues(Tmp);
- }
-
- (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)))
- {
- cInputCalcSensorValues(Tmp);
- }
- }
-}
-
-
-void cInputCalcSensorValues(UBYTE No)
-{
- UBYTE sType = IOMapInput.Inputs[No].SensorType;
-
- switch(sType)
- {
- case SWITCH:
- case TEMPERATURE:
- case REFLECTION:
- case ANGLE:
- case LIGHT_ACTIVE:
- case LIGHT_INACTIVE:
- case SOUND_DB:
- case SOUND_DBA:
- case CUSTOM:
- {
- UWORD InputVal;
-
- if (sType == CUSTOM) {
- /* 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;
-
- if (sType == REFLECTION)
- {
- cInputCalcFullScale(&InputVal, REFLECTIONSENSORMIN, REFLECTIONSENSORPCTDYN, TRUE);
- }
- else if (sType == TEMPERATURE)
- {
- if (InputVal < 290)
- InputVal = 290;
- else if (InputVal > 928)
- InputVal = 928;
- InputVal = cInputTempConv(InputVal - 290);
- InputVal = InputVal + 200;
- InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900);
- }
- else if (sType == LIGHT_ACTIVE || sType == LIGHT_INACTIVE)
- {
- cInputCalcFullScale(&InputVal, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE);
- }
- else if (sType == SOUND_DB || sType == SOUND_DBA)
- {
- cInputCalcFullScale(&InputVal, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE);
- }
- else if (sType == CUSTOM)
- {
- 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))
- {
-
- /* 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;
- UBYTE Sample;
-
- if (0 == Slope)
- {
-
- /* This is absolute measure method */
- if (NewSensorRaw > THRESHOLD_FALSE)
- {
- PresentBoolean = FALSE;
- }
- else
- {
- if (NewSensorRaw < THRESHOLD_TRUE)
- {
- PresentBoolean = TRUE;
- }
- }
- }
- else
- {
-
- /* This is dynamic measure method */
- if (NewSensorRaw > (ACTUAL_AD_RES - Slope))
- {
- PresentBoolean = FALSE;
- }
- else
- {
- if (NewSensorRaw < Slope)
- {
- PresentBoolean = TRUE;
- }
- else
- {
- Delta = *pOldSensorRaw - NewSensorRaw;
- if (Delta < 0)
- {
- if (-Delta > Slope)
- {
- PresentBoolean = FALSE;
- }
- }
- else
- {
- if (Delta > Slope)
- {
- PresentBoolean = TRUE;
- }
- }
- }
- }
- }
- *pOldSensorRaw = NewSensorRaw;
-
- switch(Mode)
- {
-
- case RAWMODE:
- {
- *pSensorValue = NewSensorRaw;
- }
- break;
-
- case BOOLEANMODE:
- {
- *pSensorValue = PresentBoolean;
- }
- break;
-
- case TRANSITIONCNTMODE:
- {
- if ((*pDebounce) > 0)
- {
- (*pDebounce)--;
- }
- else
- {
- if (*pBoolean != PresentBoolean)
- {
- (*pDebounce) = DEBOUNCERELOAD;
- (*pSensorValue)++;
- }
- }
- }
- break;
-
- case PERIODCOUNTERMODE:
- {
- if ((*pDebounce) > 0)
- {
- (*pDebounce)--;
- }
- else
- {
- if (*pBoolean != PresentBoolean)
- {
- (*pDebounce) = DEBOUNCERELOAD;
- *pBoolean = PresentBoolean;
- if (++(*pEdgeCnt) > 1)
- {
- if (PresentBoolean == 0)
- {
- (*pEdgeCnt) = 0;
- (*pSensorValue)++;
- }
- }
- }
- }
- }
- break;
-
- case PCTFULLSCALEMODE:
- {
-
- /* Output is 0-100 pct */
- *pSensorValue = ((NewSensorRaw) * 100)/SENSOR_RESOLUTION;
- }
- break;
-
- case FAHRENHEITMODE:
- {
-
- /* Fahrenheit mode goes from -40 to 158 degrees */
- *pSensorValue = (((ULONG)(NewSensorRaw) * 900L)/SENSOR_RESOLUTION) - 200;
- *pSensorValue = ((180L * (ULONG)(*pSensorValue))/100L) + 320;
- }
- break;
-
- case CELSIUSMODE:
- {
-
- /* Celsius mode goes from -20 to 70 degrees */
- *pSensorValue = (((ULONG)(NewSensorRaw * 900L)/SENSOR_RESOLUTION) - 200);
- }
- break;
-
- case ANGLESTEPSMODE:
- {
- *pBoolean = PresentBoolean;
-
- if (NewSensorRaw < ANGLELIMITA)
- {
- Sample = 0;
- }
- else
- {
- if (NewSensorRaw < ANGLELIMITB)
- {
- Sample = 1;
- }
- else
- {
- if (NewSensorRaw < ANGLELIMITC)
- {
- Sample = 2;
- }
- else
- {
- Sample = 3;
- }
- }
- }
-
- switch (*LastAngle)
- {
- case 0 :
- {
- if (Sample == 1)
- {
- if ((*pSampleCnt) >= ROT_SLOW_SPEED )
- {
-
- if (++(*pSampleCnt) >= (ROT_SLOW_SPEED + ROT_OV_SAMPLING))
- {
- (*pSensorValue)++;
- (*LastAngle) = Sample;
- }
- }
- else
- {
- (*pSensorValue)++;
- (*LastAngle) = Sample;
- }
- }
- if (Sample == 2)
- {
- (*pSensorValue)--;
- (*LastAngle) = Sample;
- }
- if (Sample == 0)
- {
- if ((*pSampleCnt) < ROT_SLOW_SPEED)
- {
- (*pSampleCnt)++;
- }
- }
- }
- break;
- case 1 :
- {
- if (Sample == 3)
- {
- (*pSensorValue)++;
- (*LastAngle) = Sample;
- }
- if (Sample == 0)
- {
- (*pSensorValue)--;
- (*LastAngle) = Sample;
- }
- (*pSampleCnt) = 0;
- }
- break;
- case 2 :
- {
- if (Sample == 0)
- {
- (*pSensorValue)++;
- (*LastAngle) = Sample;
- }
- if (Sample == 3)
- {
- (*pSensorValue)--;
- (*LastAngle) = Sample;
- }
- (*pSampleCnt) = 0;
- }
- break;
- case 3 :
- {
- if (Sample == 2)
- {
- if ((*pSampleCnt) >= ROT_SLOW_SPEED)
- {
-
- if (++(*pSampleCnt) >= (ROT_SLOW_SPEED + ROT_OV_SAMPLING))
- {
- (*pSensorValue)++;
- (*LastAngle) = Sample;
- }
- }
- else
- {
- (*pSensorValue)++;
- (*LastAngle) = Sample;
- }
- }
- if (Sample == 1)
- {
- (*pSensorValue)--;
- (*LastAngle) = Sample;
- }
- if (Sample == 3)
- {
- if ((*pSampleCnt) < ROT_SLOW_SPEED)
- {
- (*pSampleCnt)++;
- }
- }
- }
- break;
- }
- }
- }
-
- *pBoolean = PresentBoolean;
-}
-
-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 *pType, UBYTE OldType)
-{
-
- VarsInput.InvalidTimer[Port] = INVALID_RELOAD_NORMAL;
-
- /* If old type is color sensor in color lamp mode then turn off leds */
- switch (OldType)
- {
- 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:
- {
- dInputSetInactive(Port);
- dInputSetDirInDigi0(Port);
- dInputSetDirInDigi1(Port);
- }
- break;
-
- case REFLECTION:
- case ANGLE:
- {
- dInputSetActive(Port);
- dInputClearDigi0(Port);
- dInputClearDigi1(Port);
- }
- break;
-
- case LIGHT_ACTIVE:
- {
- dInputSetInactive(Port);
- dInputSetDigi0(Port);
- dInputClearDigi1(Port);
- }
- break;
-
- case LIGHT_INACTIVE:
- {
- dInputSetInactive(Port);
- dInputClearDigi0(Port);
- dInputClearDigi1(Port);
- }
- break;
-
- case SOUND_DB:
- {
- VarsInput.InvalidTimer[Port] = INVALID_RELOAD_SOUND;
- dInputSetInactive(Port);
- dInputSetDigi0(Port);
- dInputClearDigi1(Port);
- }
- break;
-
- case SOUND_DBA:
- {
- VarsInput.InvalidTimer[Port] = INVALID_RELOAD_SOUND;
- dInputSetInactive(Port);
- dInputClearDigi0(Port);
- dInputSetDigi1(Port);
- }
- break;
-
- case CUSTOM:
- {
- cInputSetupCustomSensor(Port);
- }
- break;
-
- case LOWSPEED:
- {
- 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 COLORFULL:
- case COLORRED:
- case COLORGREEN:
- case COLORBLUE:
- 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:
- {
- }
- break;
- }
-}
-
-void cInputSetupCustomSensor(UBYTE Port)
-{
- if ((IOMapInput.Inputs[Port].DigiPinsDir) & 0x01)
- {
- 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
- {
- dInputSetDirInDigi1(Port);
- }
-
- if (CUSTOMACTIVE == (IOMapInput.Inputs[Port].CustomActiveStatus))
- {
- dInputSetActive(Port);
- }
- else
- {
- if (CUSTOM9V == (IOMapInput.Inputs[Port].CustomActiveStatus))
- {
- dInputSet9v(Port);
- }
- else
- {
- dInputSetInactive(Port);
- }
- }
-}
-
-
-SWORD cInputTempConv(UWORD InputVal)
-{
- static const long long TempCoeff[] = { -5425ll, 9261399ll, -6686663252ll,
- 2573629857807ll, -822478326197838ll, 195856762719738784ll };
- const unsigned int TempCoeffShift = 48;
- /* Replace the original table with polynomial. */
- int i;
- long long Input = InputVal;
- long long Output = TempCoeff[0];
- for (i = 1; i < sizeof TempCoeff / sizeof TempCoeff[0]; i++)
- Output = Output * Input + TempCoeff[i];
- /* Round. */
- return Output + (1ll << TempCoeffShift - 1) >> TempCoeffShift;
-}
-
-
-UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus)
-{
-
- *pInitStatus = FALSE;
- switch(VarsInput.VarsColor[Port].ColorInitState)
- {
- 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:
- {
-
- dInputClearDigi0(Port);
-
- /* Clear clock for 100mS - use pit timer*/
- dInputClearColor100msTimer(Port);
- VarsInput.VarsColor[Port].ColorInitState++;
- }
- break;
- case 4:
- {
-
- /* Wait 100mS */
- if (dInputChkColor100msTimer(Port))
- {
- VarsInput.VarsColor[Port].ColorInitState += 1;
- }
- }
- break;
- case 5:
- {
- UBYTE TmpType;
-
- if (COLOREXIT == IOMapInput.Inputs[Port].SensorType)
- {
- TmpType = COLORNONE;
- }
- else
- {
- TmpType = IOMapInput.Inputs[Port].SensorType;
- }
- 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))))
- {
- 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 ((CrcCheck != Crc))
- {
-
- /* incorrect!!! try again */
- VarsInput.VarsColor[Port].ColorInitState = 0;
- VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR;
- }
- else
- {
-
- /* 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 cInputCalibrateColor(COLORSTRUCT *pC, UWORD *pNewVals)
-{
- UBYTE CalRange;
-
- if ((pC->ADRaw[BLANK]) < pC->CalLimits[1])
- {
- CalRange = 2;
- }
- else
- {
- if ((pC->ADRaw[BLANK]) < pC->CalLimits[0])
- {
- CalRange = 1;
- }
- else
- {
- CalRange = 0;
- }
- }
-
- pNewVals[RED] = 0;
- if ((pC->ADRaw[RED]) > (pC->ADRaw[BLANK]))
- {
- pNewVals[RED] = (UWORD)(((ULONG)((pC->ADRaw[RED]) - (pC->ADRaw[BLANK])) * (pC->Calibration[CalRange][RED])) >> 16);
- }
-
- pNewVals[GREEN] = 0;
- if ((pC->ADRaw[GREEN]) > (pC->ADRaw[BLANK]))
- {
- pNewVals[GREEN] = (UWORD)(((ULONG)((pC->ADRaw[GREEN]) - (pC->ADRaw[BLANK])) * (pC->Calibration[CalRange][GREEN])) >> 16);
- }
-
- pNewVals[BLUE] = 0;
- if ((pC->ADRaw[BLUE]) > (pC->ADRaw[BLANK]))
- {
- 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();
-}
-