' ========================================================================= ' ' File...... TrafficBuster.bsp ' Purpose... Prototype This: Traffic Buster Control Module ' Author.... J. Grand ' ' {$STAMP BS2p} ' {$PBASIC 2.5} ' ' ========================================================================= ' -----[ Program Description ]--------------------------------------------- ' This program controls the entire Traffic Buster control/interface. ' ' There are multiple modes, depending on the compile-time setting: ' ' * Manual using wireless PS2 controller ' * Self-parking using sensor data and Zoz PC processing ' * Dead reckoning capture (using PS2 controller) and playback via Zoz PC ' ' Manual control of the Traffic Buster uses a wireless Sony PlayStation 2 ' controller. This code assumes that the clock signal is inverted between ' the Stamp and the controller (which we take care of in hardware) to allow ' simpler [less sophisticated] interface with SHIFTOUT AND SHIFTIN. ' ' Note: Portions of the PS2 interface code is based on previous work by ' Aaron Dahlen. ' ' -----[ Revision History ]------------------------------------------------ ' -----[ Select the control interface ]-------------------------------------------------- #DEFINE PS2 = 1 '#DEFINE ZOZ_SENSOR = 1 '#DEFINE ZOZ_DEAD_CAPTURE = 1 '#DEFINE ZOZ_DEAD_PLAYBACK = 1 ' -----[ I/O Definitions ]------------------------------------------------- ' Main I/O port ValveOut0 PIN 0 ' wheels flip out, PS2 circle ValveOut1 PIN 1 ' lift car down, PS2 X 'ValveOut2 PIN 2 ' wheels flip in, PS2 square (unused for safety) ValveOut3 PIN 3 ' lift car up, PS2 triangle ValveOut4 PIN 4 ' hydraulic motor starter, PS2 R1 (both motors are tied in parallel in the truck) 'ValveOut5 PIN 6 ' hydraulic motor starter, PS2 R2 PotClk PIN 13 PotRst PIN 14 PotDQ PIN 15 ' OUT15 ' Auxiliary I/O port AdcClk PIN 0 ' ADC clock AdcDta PIN 1 ' ADC data line AdcCSFront PIN 2 ' ADC chip selects AdcCSBack PIN 3 Ping PIN 8 KEYFOB PIN 9 PC_RX PIN 10 ' Serial input FROM PC (TTL-level) PC_TX PIN 11 ' Serial OUTPUT TO PC PsxDat PIN 12 ' PSX joystick interface PsxCmd PIN 13 PsxAtt PIN 14 PsxClk PIN 15 ' -----[ Constants ]------------------------------------------------------- Invert CON 1 ' inverted clock signal Direct CON 0 ' no inverter in clock line ClockMode CON Invert #SELECT $STAMP #CASE BS2, BS2E, BS2PE T1200 CON 813 T2400 CON 396 T4800 CON 188 T9600 CON 84 T19K2 CON 32 T38K4 CON 6 #CASE BS2SX, BS2P T1200 CON 2063 T2400 CON 1021 T4800 CON 500 T9600 CON 240 T19K2 CON 110 T38K4 CON 45 #ENDSELECT SevenBit CON $2000 Inverted CON $4000 Open CON $8000 PC_Baud CON T9600 ' Parallax Ping ultrasonic sensor #SELECT $STAMP #CASE BS2, BS2E Trigger CON 5 ' trigger pulse = 10 uS Scale CON $200 ' raw x 2.00 = uS #CASE BS2SX, BS2P, BS2PX Trigger CON 13 Scale CON $0CD ' raw x 0.80 = uS #CASE BS2PE Trigger CON 5 Scale CON $1E1 ' raw x 1.88 = uS #ENDSELECT RawToCm CON 2257 ' 1 / 29.034 (with **) IsHigh CON 1 ' for PULSOUT IsLow CON 0 PingMax CON 20 ' cm ' GP2D12 infrared sensor Xspan CON 5 ' 5 cm per data point IRMin CON 10 ' cm IRMax CON 60 ' PlayStation 2 button constants ' psxThumbL PS2_Select CON $01 PS2_Left_Thumb CON $02 PS2_Right_Thumb CON $04 PS2_Start CON $08 PS2_Dpad_Up CON $10 PS2_Dpad_Right CON $20 PS2_Dpad_Down CON $40 PS2_Dpad_Left CON $80 ' psxThumbR PS2_Left_Trigger2 CON $01 PS2_Right_Trigger2 CON $02 PS2_Left_Trigger1 CON $04 PS2_Right_Trigger1 CON $08 PS2_Triangle CON $10 PS2_Circle CON $20 PS2_X CON $40 PS2_Square CON $80 ' Airtrax Airtrax_Voltage_Min CON $10 Airtrax_Voltage_Max CON $A0 ' -----[ Variables ]------------------------------------------------------- idx VAR NIB ' loop counter psxOut VAR BYTE ' byte to controller psxIn VAR BYTE ' byte from controller ' joystick packet psxID VAR BYTE ' controller ID psxThumbL VAR BYTE ' left thumb buttons psxThumbR VAR BYTE ' right thumb buttons psxStatus VAR BYTE ' status ($5A) psxJoyRX VAR BYTE ' r joystick - X axis psxJoyRY VAR BYTE ' r joystick - Y axis psxJoyLX VAR BYTE ' l joystick - X axis psxJoyLY VAR BYTE ' l joystick - Y axis ' airtrax X, Y, Z (twist) analog motion values airtrax_X VAR BYTE airtrax_Y VAR BYTE airtrax_Z VAR BYTE ' hydraulic control flags for dead reckoning airtrax_unfold VAR BYTE airtrax_lift VAR BYTE ' DS1867 dual digital potentiometer Stack VAR BIT OutByte VAR BYTE #IF ZOZ_SENSOR #THEN ' sensor data (values in centimeters from nearest object) infrared_front VAR BYTE infrared_back VAR BYTE ultrasonic VAR BYTE result VAR BYTE ' adc result cVolts VAR WORD ' 0.01 volts test1 VAR BYTE ' test values for test2 VAR BYTE ' interpolation slope VAR WORD ' mV/cm between test points addr VAR BIT ' front = 0, back = 1 #ENDIF ' -----[ EEPROM Data ]----------------------------------------------------- Vout DATA 251, 179, 139, 114, 97 DATA 85, 76, 67, 62, 57 DATA 53, 50, 48, 46, 43 DATA 0 ' -----[ Initialization ]-------------------------------------------------- Setup: MAINIO LOW ValveOut0 LOW ValveOut1 'LOW ValveOut2 LOW ValveOut3 LOW ValveOut4 'LOW ValveOut5 OUTPUT PotClk OUTPUT PotRst OUTPUT PotDQ Stack = 0 ' set stack bit to 0 (we don't want stacked output) ' set airtrax output to idle state (~2.5V) airtrax_X = $80 airtrax_Y = $80 airtrax_Z = $80 GOSUB DS1867_WriteVal AUXIO ' use auxiliary port HIGH AdcCSFront HIGH AdcCSBack INPUT KEYFOB HIGH PsxAtt ' deselect PSX controller OUTPUT PsxCmd PsxClk = ~ClockMode ' release clock OUTPUT PsxClk ' make clock an output DEBUG CLS, CR, "Traffic Buster Control Module", CR, CR ' -----[ Program Code ]---------------------------------------------------- Main: IF (KEYFOB = 0) THEN ' set all output potentiometers to AirTrax idle state (0x80) airtrax_X = 127 airtrax_Y = 127 airtrax_Z = 127 GOSUB DS1867_WriteVal AUXIO GOTO Main ' wait for keyfob to be pressed in order to start our program ENDIF DEBUG "GO!", CR, CR ' -----[ PlayStation 2 Joystick Control (Manual, Wireless) ]------ #IF PS2 #THEN DO GOSUB Get_PSX_Packet IF ((psxStatus = $5A) AND (psxId <> $41)) THEN ' if we can see a PS2 analog controller 'DEBUG "PS2 wireless control!", CR, CR psxThumbL = psxThumbL ^ $FF ' invert psxThumbR = psxThumbR ^ $FF 'DEBUG IHEX2 psxId, " (", IHEX2 psxStatus, ") ", BIN8 psxThumbL, " ", BIN8 psxThumbR, " " 'DEBUG DEC3 psxJoyLX, " ", DEC3 psxJoyLY, " ", DEC3 psxJoyRX, " ", DEC3 psxJoyRY, " ", CR ' Airtrax manual control, left analog stick: X & Y axis, right analog stick: Z axis (twist) 'airtrax_X = psxJoyLX 'airtrax_Y = psxJoyLY 'airtrax_Z = psxJoyRX ' reduce sensitivity of joystick controls airtrax_X = ((psxJoyLX - $80) / 4) + $80 airtrax_Y = ((psxJoyLY - $80) / 4) + $80 airtrax_Z = ((psxJoyRX - $80) / 4) + $80 ' we need to avoid the floor and ceiling error voltages (less than 0.5V and greater than 4.5V) IF (airtrax_X < Airtrax_Voltage_Min) THEN airtrax_X = Airtrax_Voltage_Min IF (airtrax_X > Airtrax_Voltage_Max) THEN airtrax_X = Airtrax_Voltage_Max IF (airtrax_Y < Airtrax_Voltage_Min) THEN airtrax_Y = Airtrax_Voltage_Min IF (airtrax_Y > Airtrax_Voltage_Max) THEN airtrax_Y = Airtrax_Voltage_Max IF (airtrax_Z < Airtrax_Voltage_Min) THEN airtrax_Z = Airtrax_Voltage_Min IF (airtrax_Z > Airtrax_Voltage_Max) THEN airtrax_Z = Airtrax_Voltage_Max 'DEBUG "X: ", DEC3 airtrax_X, " Y: ", DEC3 airtrax_Y, " Z: ", DEC3 airtrax_Z, CR airtrax_Y = -airtrax_Y GOSUB DS1867_WriteVal ' write new potentiometer values (e.g., output new analog voltages to Airtrax) MAINIO ' check button presses - ONLY ONE BUTTON SHOULD BE PRESSED AT A TIME TO PREVENT DAMAGE TO OUR HYDRAULIC SYSTEM ' wheels flip out IF (psxThumbR AND PS2_Circle) THEN DEBUG "Circle: Wheels flip out...", CR HIGH ValveOut0 ELSE LOW ValveOut0 ENDIF ' lift car down IF (psxThumbR AND PS2_X) THEN DEBUG "X: Lift car down...", CR HIGH ValveOut1 ELSE LOW ValveOut1 ENDIF ' wheels flip in 'IF (psxThumbR AND PS2_Square) THEN ' DEBUG "Square: Wheels flip in...", CR ' HIGH ValveOut2 'ELSE ' LOW ValveOut2 'ENDIF ' lift car up IF (psxThumbR AND PS2_Triangle) THEN DEBUG "Triangle: Lift car up...", CR HIGH ValveOut3 ELSE LOW ValveOut3 ENDIF ' hydraulic motor starter IF (psxThumbR AND PS2_Right_Trigger1) THEN DEBUG "Right Trigger 1: Hydraulic motor starter...", CR HIGH ValveOut4 ELSE LOW ValveOut4 ENDIF ' hydraulic motor starter #2 'IF (psxThumbR AND PS2_Right_Trigger2) THEN ' DEBUG "Right Trigger 2: Hydraulic motor starter #2...", CR ' HIGH ValveOut5 'ELSE ' LOW ValveOut5 'ENDIF ELSE airtrax_X = 127 airtrax_Y = 127 airtrax_Z = 127 GOSUB DS1867_WriteVal MAINIO LOW ValveOut0 LOW ValveOut1 'LOW ValveOut2 LOW ValveOut3 LOW ValveOut4 'LOW ValveOut5 DEBUG "No response from controller.", CR PAUSE 1000 ENDIF LOOP #ENDIF ' -----[ Zoz PC Control (Sensor/Self-Parking) ]------ #IF ZOZ_SENSOR #THEN 'DEBUG "Zoz control!", CR, CR SEROUT PC_TX, PC_Baud, ["!G"] ' send "Go!" command to Zoz DO ' process sensor information (distances in cm from nearest object)... addr = 0 GOSUB Read_GP2D12 ' read sensor GOSUB Estimate_Cm infrared_front = result 'IF (infrared_front < IRMin) THEN infrared_front = IRMin IF (infrared_front > IRMax) THEN infrared_front = 0 'DEBUG "IR Front: ", DEC infrared_front, " cm (", DEC cVolts / 100, ".", DEC2 cVolts, " volts)", CR addr = 1 GOSUB Read_GP2D12 ' read sensor GOSUB Estimate_Cm infrared_back = result 'IF (infrared_back < IRMin) THEN infrared_back = IRMin IF (infrared_back > IRMax) THEN infrared_back = 0 'DEBUG "IR Back: ", DEC infrared_back, " cm (", DEC cVolts / 100, ".", DEC2 cVolts, " volts)", CR GOSUB Get_Sonar ' get sensor value IF (ultrasonic > PingMax) THEN ultrasonic = 0 'IF (ultrasonic > 0) THEN DEBUG "Ultrasonic: ", DEC ultrasonic, " cm", CR ' ...and send it all to Zoz AUXIO SEROUT PC_TX, PC_Baud, ["!D", infrared_front, infrared_back, ultrasonic] ' and wait for acknowledgement from him containing Airtrax X, Y, and Z motion values SERIN PC_RX, PC_Baud, [WAIT("!M"), airtrax_Y, airtrax_X, airtrax_Z] airtrax_X = airtrax_X + 127 airtrax_Y = airtrax_Y + 127 airtrax_Z = airtrax_Z + 127 DEBUG "X: ", DEC3 airtrax_X, " Y: ", DEC3 airtrax_Y, " Z: ", DEC3 airtrax_Z, CR ' we need to avoid the floor and ceiling error voltages (less than 0.5V and greater than 4.5V) IF (airtrax_X < Airtrax_Voltage_Min) THEN airtrax_X = Airtrax_Voltage_Min IF (airtrax_X > Airtrax_Voltage_Max) THEN airtrax_X = Airtrax_Voltage_Max IF (airtrax_Y < Airtrax_Voltage_Min) THEN airtrax_Y = Airtrax_Voltage_Min IF (airtrax_Y > Airtrax_Voltage_Max) THEN airtrax_Y = Airtrax_Voltage_Max IF (airtrax_Z < Airtrax_Voltage_Min) THEN airtrax_Z = Airtrax_Voltage_Min IF (airtrax_Z > Airtrax_Voltage_Max) THEN airtrax_Z = Airtrax_Voltage_Max GOSUB DS1867_WriteVal ' write new potentiometer values (e.g., output new analog voltages to Airtrax LOOP #ENDIF ' -----[ Zoz PC Control (Dead Reckoning/Auto Valet - Zoz Capture) ]------ #IF ZOZ_DEAD_CAPTURE #THEN DEBUG "Recording started...", CR SEROUT PC_TX, PC_Baud, ["!C", $00, $00, $00, $00, $00] ' send "start recording" command to Zoz SEROUT PC_TX, PC_Baud, ["!R", $00, $00, $00, $00, $00] ' send "start recording" command to Zoz DO GOSUB Get_PSX_Packet IF ((psxStatus = $5A) AND (psxId <> $41)) THEN ' if we can see a PS2 analog controller 'DEBUG "PS2 wireless control!", CR, CR psxThumbL = psxThumbL ^ $FF ' invert psxThumbR = psxThumbR ^ $FF 'DEBUG IHEX2 psxId, " (", IHEX2 psxStatus, ") ", BIN8 psxThumbL, " ", BIN8 psxThumbR, " " 'DEBUG DEC3 psxJoyLX, " ", DEC3 psxJoyLY, " ", DEC3 psxJoyRX, " ", DEC3 psxJoyRY, " ", CR ' Airtrax manual control, left analog stick: X & Y axis, right analog stick: Z axis (twist) 'airtrax_X = psxJoyLX 'airtrax_Y = psxJoyLY 'airtrax_Z = psxJoyRX ' reduce sensitivity of joystick controls airtrax_X = ((psxJoyLX - $80) / 4) + $80 airtrax_Y = ((psxJoyLY - $80) / 4) + $80 airtrax_Z = ((psxJoyRX - $80) / 4) + $80 ' we need to avoid the floor and ceiling error voltages (less than 0.5V and greater than 4.5V) IF (airtrax_X < Airtrax_Voltage_Min) THEN airtrax_X = Airtrax_Voltage_Min IF (airtrax_X > Airtrax_Voltage_Max) THEN airtrax_X = Airtrax_Voltage_Max IF (airtrax_Y < Airtrax_Voltage_Min) THEN airtrax_Y = Airtrax_Voltage_Min IF (airtrax_Y > Airtrax_Voltage_Max) THEN airtrax_Y = Airtrax_Voltage_Max IF (airtrax_Z < Airtrax_Voltage_Min) THEN airtrax_Z = Airtrax_Voltage_Min IF (airtrax_Z > Airtrax_Voltage_Max) THEN airtrax_Z = Airtrax_Voltage_Max ' check button presses IF (psxThumbR AND PS2_Circle) THEN 'DEBUG "Circle: Wheels flip out...", CR airtrax_unfold = 1 ELSE airtrax_unfold = 0 ENDIF IF (psxThumbR AND PS2_X) THEN ' lift car down 'DEBUG "X: Lift car down...", CR airtrax_lift = -1 ELSEIF (psxThumbR AND PS2_Triangle) THEN ' lift car up 'DEBUG "Triangle: Lift car up...", CR airtrax_lift = 1 ELSE airtrax_lift = 0 ENDIF DEBUG "X: ", DEC3 airtrax_X, " Y: ", DEC3 airtrax_Y, " Z: ", DEC3 airtrax_Z, " Unfold: ", DEC3 airtrax_unfold, " Lift: ", DEC3 airtrax_lift, CR SEROUT PC_TX, PC_Baud, ["!M", airtrax_X, airtrax_Y, airtrax_Z, airtrax_unfold, airtrax_lift] ' send movement/control data to Zoz 'SERIN PC_RX, PC_Baud, [WAIT("!M"), airtrax_X, airtrax_Y, airtrax_Z, airtrax_unfold, airtrax_lift] ' send movement/control data to Zoz 'DEBUG "X: ", DEC3 airtrax_X, " Y: ", DEC3 airtrax_Y, " Z: ", DEC3 airtrax_Z, " Unfold: ", DEC3 airtrax_unfold, " Lift: ", DEC3 airtrax_lift, CR ENDIF LOOP UNTIL ((psxThumbR AND PS2_Left_Trigger1) > 0) DEBUG "Recording stopped...", CR SEROUT PC_TX, PC_Baud, ["!r", $00, $00, $00, $00, $00] ' "stop recording" command to Zoz GOTO Main #ENDIF ' -----[ Zoz PC Control (Dead Reckoning/Auto Valet - Zoz Playback) ]------ #IF ZOZ_DEAD_PLAYBACK #THEN DEBUG "Starting dead reckoning playback...", CR SEROUT PC_TX, PC_Baud, ["!P", $00, $00, $00, $00, $00] DO ' get Airtrax motion and control values from Zoz SERIN PC_RX, PC_Baud, [WAIT("!M"), airtrax_Y, airtrax_X, airtrax_Z, airtrax_unfold, airtrax_lift] airtrax_X = airtrax_X + 127 airtrax_Y = airtrax_Y + 127 airtrax_Z = airtrax_Z + 127 DEBUG "X: ", DEC3 airtrax_X, " Y: ", DEC3 airtrax_Y, " Z: ", DEC3 airtrax_Z, " Unfold: ", DEC3 airtrax_unfold, " Lift: ", DEC3 airtrax_lift, CR GOSUB DS1867_WriteVal ' write new potentiometer values (e.g., output new analog voltages to Airtrax IF (airtrax_unfold = 1) THEN DEBUG "Circle: Wheels flip out...", CR HIGH ValveOut4 HIGH ValveOut0 ELSE LOW ValveOut0 LOW ValveOut4 ENDIF IF (airtrax_lift = -1) THEN DEBUG "X: Lift car down...", CR HIGH ValveOut4 HIGH ValveOut1 ELSEIF (airtrax_lift = 1) THEN DEBUG "Triangle: Lift car up...", CR HIGH ValveOut4 HIGH ValveOut3 ELSE LOW ValveOut1 LOW ValveOut4 ENDIF LOOP #ENDIF END ' -----[ Subroutines ]----------------------------------------------------- ' -----[ PlayStation 2 controller ]---------------------------------------- ' This routine manually creates the clock signal, ' so it can be used with a direct (via 220 ohm resistor) ' connection to the clock input ' ' Execution time on BS2 is ~145 ms. Get_PSX_Packet: AUXIO LOW PsxAtt ' select controller psxOut = $01 : GOSUB PSX_TxRx ' send "start" psxOut = $42 : GOSUB PSX_TxRx ' send "get data" psxId = psxIn ' save controller type psxOut = $00 : GOSUB PSX_TxRx psxStatus = psxIn ' should be $5A ("ready") GOSUB PSX_TxRx : psxThumbL = psxIn ' get PSX data GOSUB PSX_TxRx : psxThumbR = psxIn GOSUB PSX_TxRx : psxJoyRX = psxIn GOSUB PSX_TxRx : psxJoyRY = psxIn GOSUB PSX_TxRx : psxJoyLX = psxIn GOSUB PSX_TxRx : psxJoyLY = psxIn HIGH PsxAtt ' deselect controller RETURN ' Transmit psxOut to, and receive psxIn from the ' PSX controller PSX_TxRx: FOR idx = 0 TO 7 PsxCmd = psxOut.LOWBIT(idx) ' setup command bit PsxClk = ClockMode ' clock the bit psxIn.LOWBIT(idx) = PsxDat ' get data bit PsxClk = ~ClockMode ' release clock NEXT RETURN ' This routine REQUIRES inverted clock signal from ' Stamp to PSX controller 'Get_PSX_Buttons: ' IF (ClockMode = Direct) THEN Get_PSX_Packet ' redirect if not inverted ' LOW PsxAtt ' SHIFTOUT PsxCmd, PsxClk, LSBFIRST, [$01, $42] ' SHIFTIN PsxDat, PsxClk, LSBPOST, [psxThumbL, psxThumbL, psxThumbR] ' psxId = $41 ' HIGH PsxAtt ' RETURN ' -----[ DS1867 ]----------------------------------------------------- DS1867_WriteVal: MAINIO ' initial setup LOW PotRst LOW PotClk HIGH PotRst ' bring RST high while CLK is low ' output stack bit to DQ, pot 1 OUT5 = Stack GOSUB DS1867_ClockPulse ' output potentiometer values, pot 1 OutByte = $00 ' unused GOSUB DS1867_OutByte OutByte = airtrax_Z GOSUB DS1867_OutByte ' output stack bit to DQ, pot 0 OUT5 = Stack GOSUB DS1867_ClockPulse ' output potentiometer values, pot 0 OutByte = airtrax_Y GOSUB DS1867_OutByte OutByte = airtrax_X GOSUB DS1867_OutByte ' done LOW PotRst RETURN DS1867_OutByte: ' output 8 bits, beginning with the most significant bit to DQ 'DEBUG CR, "OutByte ($", HEX2 OutByte, "): " FOR idx = 0 TO 7 'DEBUG DEC1 OutByte.BIT7 OUT15 = OutByte.BIT7 GOSUB DS1867_ClockPulse OutByte = OutByte << 1 ' shift one place to the left NEXT RETURN DS1867_ClockPulse: HIGH PotClk LOW PotClk RETURN #IF ZOZ_SENSOR #THEN ' -----[ Ping Ultrasonic Sensor ]---------------------------------------- ' This subroutine triggers the Ping sonar sensor and measures ' the echo pulse. The raw value from the sensor is converted to ' microseconds based on the Stamp module in use. This value is ' divided by two to remove the return trip -- the result value is ' the distance from the sensor to the target in microseconds. Get_Sonar: AUXIO Ping = IsLow ' make trigger 0-1-0 PULSOUT Ping, Trigger ' activate sensor PULSIN Ping, IsHigh, slope ' measure echo pulse slope = slope */ Scale ' convert to uS slope = slope / 2 ' remove return trip ultrasonic = slope ** RawToCm ' convert to centimeters RETURN ' -----[ Sharp GP2D12 Infrared Sensor ]---------------------------------- ' Uses ADC0831 ADC to read voltage output from GP2D12 range sensor. Note ' that the Vref input of the ADC0831 is set to 2.55 vdc, giving 0.01 volts ' per count. We can sense from approx. 10cm to 60cm. Read_0831: AUXIO IF (addr = 0) THEN LOW AdcCSFront ' enable ADC0831 HIGH AdcCSBack ELSE LOW AdcCSBack HIGH AdcCSFront ENDIF SHIFTIN AdcDta, AdcClk, MSBPOST, [result\9] ' read the voltage HIGH AdcCSFront HIGH AdcCSBack RETURN Read_GP2D12: cVolts = 0 ' reset reading FOR idx = 0 TO 2 ' three reads (to filter) GOSUB Read_0831 ' get the voltage cVolts = cVolts + result ' accumulate PAUSE 30 NEXT cVolts = cVolts / 3 ' average the readings RETURN Estimate_Cm: FOR idx = 0 TO 15 ' search table for location READ (Vout + idx), test2 ' read table value IF (test2 <= cVolts) THEN EXIT ' found position NEXT SELECT idx CASE 0 result = 10 ' fix to minimum range CASE 1 TO 14 ' calculate range result = 10 + (5 * idx) IF (test2 < cVolts) THEN ' estimate through interpolation READ (Vout + idx - 1), test1 ' get other border value slope = (test1 - test2) * 10 / Xspan ' determine slope between points result = result - ((cvolts - test2) * 10 / slope) ENDIF CASE 15 result = 80 ' fix to maximum range ENDSELECT RETURN #ENDIF