'=========Define names for LCD instructions, bps=======
CLRTRM con 12 ' Clear entire LCD screen.
ESC con 27 ' Escape character

' Uncomment (remove the ' mark from) the appropriate
' baud rate to match 6-144 baud setting. Comment
' out the others.
BAUD con I4800 ' 9600 baud, BS2.
'BAUD con $0BC ' 4800 baud, BS2.
'BAUD con $18D ' 2400 baud, BS2.
'BAUD con $32D ' 1200 baud, BS2.

'BAUD con $0F0 ' 9600 baud, BS2-SX.
'BAUD con $1F4 ' 4800 baud, BS2-SX.
'BAUD con $3FD ' 2400 baud, BS2-SX.
'BAUD con $80F ' 1200 baud, BS2-SX.

' Configuration constants==============================
'
B_96 con 0 ' 6-144 config: 9600 baud
B_48 con 1 ' 6-144 config: 4800 baud
B_24 con 2 ' 6-144 config: 2400 baud
B_12 con 3 ' 6-144 config: 1200 baud

CURSOFF con 0 ' Cursor off at startup
CURSUL con 8 ' Cursor = underline
CURSBLK con 12 ' Cursor = block

BL_OFF con 0 ' Backlight off at startup
BL_ON con 16 ' Backlight on

EEWRITE con 0 ' OK to write EEPROM
EELOCK con 32 ' Write-protect EEPROM

BLANK con 0 ' Screen blank at startup
SPLASH con 64 ' Show EEPROM screen 0 at start

'=========Define variable(s)===========================
bpsFlags var byte ' Baud rate and bit flags.
pwrDelay var byte ' Delay in seconds after startup.
bellDur var byte ' Buzzer duration, 0.5-ms units
keyInt var byte ' Key scan interval, 0.5-ms units
keyBell var byte ' Key beep duration, 0.5-ms units
keyStr var byte ' Key strobe duration, 0.5-ms units

' =================Begin configuration=================
pause 1000

serout 6,BAUD,[ESC,"W"] ' ESC-W starts config process.

' In the instruction below, list the characters you
' want returned by the keypad, starting at the lower
' left and working bottom-to-top. There must be 16
' entries, even if your keypad is smaller than 16
' keys (pad the unused positions with any character--
' say Xs--to make sure there are 16 entries.
serout 6,BAUD,["ABCDEFGHIJKLMNOP"]
bpsFlags = B_48 + CURSOFF+ BL_ON+ EEWRITE+ BLANK

' Now add up the option values listed above under
' Configuration Constants in bpsFlags:

' Next, set the various timing values. Change these
' settings as desired. The values listed below work
' well with the example programs.
pwrDelay = 0 ' Delay in seconds after startup.
bellDur = 600 ' Buzzer duration, 0.5-ms units
keyInt = 64 ' Key scan interval, 0.5-ms units
keyBell = 0 ' Key beep duration, 0.5-ms units 'Switch off auto key bleep. In app instead
keyStr = 200 ' Key strobe duration, 0.5-ms units

' Now send to the 6-144. The instruction ..rep "x"\10.. just
' pads the unused bytes of the configuration memory with
' an arbitrary value, "x".
serout 6,BAUD,[bpsFlags,pwrDelay,bellDur,keyInt,keyBell,keyStr,rep "x"\10]

debug [6,8]
' Done.

'This app is intended for Simon Rafferty's
'DIY Air suspension system. April 2005

'Version 4.6

'The main difference between this and previous versions is 'improved articulation control. I have found that self-'levelling and articulation can be mutually exclusive.
'My solution is to 'gate' the levelling. Off road, in some 'modes it will only correct the lean if the angle is
'greater than a certain value. Within that angle. all it
'does is try to keep the pressure the same on each wheel.

'Mappings:
'Front Left:
'P9 = Up, P8 = Down, AX1 Pressure
'Front Right:
'P11 = Up, P10 = Down, AX0 Pressure
'Rear Left:
'P1 = Up, P0 = Down, AX2 Pressure
'Rear Right:
'P3 = Up, P2 = Down, AX3 Pressure

'Lean Control - P4
'Height Control - P5

'Push button 1 - P6
'Push Button 2 - P7

'Inclinometer 1 - P12
'Inclinometer 2 - P13

'Serial In/Out P14 = Out, P15 = In

'Hysteresis i.e the band of pressure values assumed to be equal to demand - helps stop
'short cycling and oscilation. Needs to be small enough that vehicle does not lean
DefaultHysteresis CON 9 'This may be overriden by EEProm value

Hysteresis VAR BYTE 'Needs to be altered under some circumstances

'Max & Min Pressures (in PSI)

MinFNs VAR Byte 'These are set from EEProm
MinFOs VAR Byte
MinRNs VAR Byte
MinROs VAR Byte

MinFN VAR Byte
MinFO VAR Byte
MinRN VAR Byte
MinRO VAR Byte

IgnoreScale var byte

SetScaleFN VAR SWORD
SetScaleFO VAR SWORD
SetScaleRN VAR SWORD
SetScaleRO VAR SWORD

SetZeroFN var SWORD
SetZeroFO var SWORD
SetZeroRN var SWORD
SetZeroRO var SWORD

PressureMax CON 100

'When auto-levelling, bolus pulse length is variable.
PulseLen var BYTE

TiltGain VAR Byte

RTilt Var BYTE

'Define Input & Output pins
TiltSensor1 CON 12 'Gives values in the +/-200 range
TiltSensor2 CON 13 'Gives values in the +/-200 range

'AutoTilts var sword

SetHeight var word
SetTilt var SByte
SetTiltFR var sByte
AutoTilt var SWORD

'F ront, R ear, N earside, O ffside, I nlet, E xhaust
ValveRNI CON 1
ValveRNE CON 0
ValveROI CON 3
ValveROE CON 2

ValveFNI CON 9
ValveFNE CON 8
ValveFOI CON 11
ValveFOE CON 10

OnRoad CON 0

'Repository for last pressure readings. Use GetPressures to read
ANPressureFN VAR SWORD 'Front Pressure
ANPressureFO VAR SWORD 'Front Pressure
ANPressureRN VAR SWORD 'Rear Pressure
ANPressureRO VAR SWORD 'Rear Pressure

PrFN VAR SWORD(10)
PrFO VAR SWORD(10)
PrRN VAR SWORD(10)
PrRO VAR SWORD(10)

TiltMean VAR SWORD(10)

MeanCnt VAR BYTE
TiltCnt var BYTE

PressureValid VAR Byte

'Demand Pressures
PressureFN VAR SWORD
PressureFO VAR SWORD
PressureRN VAR SWORD
PressureRO VAR SWORD

DiffFN VAR SWORD
DiffFO VAR SWORD
DiffRN VAR SWORD
DiffRO VAR SWORD

MeanHeight VAR SWORD
RearMean VAR SWORD

FLean VAR SWORD
RLean VAR SWORD

'Repository for Tilt Sensor readings
Tilt VAR SWORD
Lean VAR SWORD

'Difference between demand and actual pressure
DIFF VAR SWORD

nCnt VAR LONG 'General Purpose
Flag VAR BYTE

OffRoad VAR BYTE 'Change some parameters between on and off road

'=========Define names for LCD instructions, bps=======
NOCURS con 4 ' Make cursor invisible.
ULCURS con 5 ' Show underline cursor.
CLRTRM con 12 ' Clear entire LCD screen.
POSCMD con 16 ' Position cursor.
CLRCOL con 17 ' Clear column.
STAIRSUP con 128 ' Wedge-shaped symbol.
STAIRSDN con 129 ' Wedge-shaped symbol.
BIGNUM con 2 ' Begin big numbers.
LITNUM con 3 ' Begin big numbers.
ARROWRIGHT con 126
ARROWLEFT con 127
BEEP con 7
CURSOROFF con 4

SerialOut con 6
SerialIn con 7
KeyStrobe VAR IN5
Compressor VAR IN4 '=1 when compressor is running

'Set serial data rate
BAUD con I4800 ' 9600 baud, BS2.

'These two key values move between screens
KeyNext con 66
KeyPrev con 70
KeyFNI con 67
KeyFNE con 71
KeyFOI con 68
KeyFOE con 72
KeyRNI con 75
KeyRNE con 79
KeyROI con 76
KeyROE con 80

'=========Define variable(s)===========================
KeyVal var BYTE ' Numeric value input from keys.
ScreenID var Byte 'Which screen is currently displayed
DiffFR var Word
ColFR var SWord
PosFR var SWORD
LastPosFR var Byte
ColLR var SWord
PosLR var SWORD
LastPosLR var Byte

LastSetTilt var sByte
LastSetHeight var Byte

PDisp VAR Byte

ORLevelGate var sbyte

'========Program starts here===========================

'*******************************************************
'*******************************************************

serout SerialOut,BAUD,[CLRTRM] 'Clear Screen
Pause 2000
serout SerialOut,BAUD,[CLRTRM] 'Clear Screen
Pause 1000

ScreenID = 1
SetTilt = -5
SetTiltFR = 0

GOSUB DrawScreen
GOSUB ZeroVars 'Zero the moving time average variables

GOSUB SetPortStates

'Get default settings from EEProm
GOSUB GetDefaults

MainLoop:

'Key has been pressed - go get data!
GOSUB GetKeyboardData
'Data returned in KeyVal
Screen:

GOSUB UpdateScreen 'This displays stuff as appropriate and reacts to keys

GOTO MainLoop

'*******************************************************
'*******************************************************

GetDefaults:

Hysteresis = 9
SetHeight = 70

SetTilt = 0
SetScaleFN = 25
SetScaleFO = 28
SetScaleRN = 26
SetScaleRO = 26
TiltGain = 6

SetZeroFN = 31
SetZeroFO = 32
SetZeroRN = 32
SetZeroRO = 32

PulseLen = 100

Return

'*******************************************************
'*******************************************************

DrawScreen:
IF ScreenID = 1 THEN
GOSUB DrawScreen1
ENDIF
IF ScreenID = 2 THEN
GOSUB DrawScreen2
ENDIF
IF ScreenID = 3 THEN
GOSUB DrawScreen3
ENDIF
'IF ScreenID = 4 THEN
' GOSUB DrawScreen4
'ENDIF
Pause 500
RETURN

'*******************************************************
'*******************************************************

UpdateScreen:
IF ScreenID = 1 THEN
GOSUB UpdateScreen1
ENDIF
IF ScreenID = 2 THEN
GOSUB UpdateScreen2
ENDIF
IF ScreenID = 3 THEN
GOSUB UpdateScreen3
ENDIF
'IF ScreenID = 4 THEN
' GOSUB UpdateScreen4
'ENDIF

RETURN

'*******************************************************
'*******************************************************

DrawScreen1:
'This is the main screen which should look something like this:
'UP 999 999 UP
'DN *DN
'UP # UP
'DN 999 999 DN
'Where 999 is the pressure and * indicates that the button is pressed
'# is a solid block showing degree of lean / pitch
'serout SerialOut,BAUD,[CLRTRM] 'Clear Screen

'serout SerialOut,BAUD,[BIGNUM, "MANU"]
'PAUSE 200

serout SerialOut,BAUD,[CLRTRM] 'Clear Screen
serout SerialOut,BAUD,["UP UPDN DNUP UPDN DN"]

RETURN

'*******************************************************
'*******************************************************

DrawScreen2:
'This is intended more for off-roading
'< Lean Lean* >
'^ Pitch Pitch v
'UP All All DN
'\ Artic Artic /
'\ represents a ridge so in this case In RN & FO, Ex RO & FN
'serout SerialOut,BAUD,[CLRTRM] 'Clear Screen

'serout SerialOut,BAUD,[BIGNUM, "O/R"]
'PAUSE 200

serout SerialOut,BAUD,[CLRTRM] 'Clear Screen
serout SerialOut,BAUD,["< Lean Lean >^ Pitch Pitch vUP All All DN",STAIRSDN," Artic Artic ",STAIRSUP]

RETURN

'*******************************************************
'*******************************************************

DrawScreen3:
'This will be used when I figure out the auto control bit
'OnRd 888 777 OfRd
'^ Height 999 v
'000 # 000
'000 # 000

'
'000 is the pressure and # represents the current tilt sensor reading
'888 is the amount of left/right lean
'777 is the fore/aft lean

serout SerialOut,BAUD,[CLRTRM] 'Clear Screen

serout SerialOut,BAUD,["OnRd OfRdUp Dn"]

RETURN

'*******************************************************
'*******************************************************

UpdateScreen1:
'
'UP 999 999 UP
'DN *DN
'UP # UP
'DN 999 999 DN
'To display this screen, we need to get the pressures,
'The lean sensor and the key pressed if any and update.

GOSUB GetPressures:

'First update pressure etc display
If ANPressureFN > 0 Then
serout SerialOut,BAUD,[POSCMD,67,SDEC ANPressureFN," "]
EndIf
If ANPressureFO > 0 Then
serout SerialOut,BAUD,[POSCMD,78,SDEC ANPressureFO," "]
EndIf
If ANPressureRN > 0 Then
serout SerialOut,BAUD,[POSCMD,127,SDEC ANPressureRN," "]
EndIf
If ANPressureRO > 0 Then
serout SerialOut,BAUD,[POSCMD,138,SDEC ANPressureRO," "]
EndIf


'Next work out the pressure differential between front & rear
DiffFR = (((ANPressureFN + ANPressureFO) / 2) - ((ANPressureRN + ANPressureRO) / 2)) / 8
'Difference should have a range of 0 to 10
GOSUB GetTiltSensors

ColFR = DiffFR + 7
If ColFR > 14 then
ColFR = 14
Endif
IF ColFR < 0 then
ColFR = 0
Endif
ColLR = (Tilt) + 7
If ColLR > 14 Then
ColLR = 14
Endif
If ColLR < 0 Then
ColLR = 0
Endif

PosLR = 20 + ColLR + 2 + 64
PosFR = 40 + ColFR + 2 + 64


'Print a block at line & col determined by lean/pitch
serout SerialOut,BAUD,[POSCMD,LastPosLR," "]
serout SerialOut,BAUD,[POSCMD,PosLR,255,255]

serout SerialOut,BAUD,[POSCMD,LastPosFR," "]
serout SerialOut,BAUD,[POSCMD,PosFR,255,255]


LastPosLR = PosLR
LastPosFR = PosFR

IF KeyVal = 0 THEN
'Turn All valves off
GOSUB CloseAllValves
'Clear the columns representing the pressed button
ELSE
'First update display based upon the button pressed, then open the valve
GOSUB CloseAllValves
IF KeyVal = KeyFNI THEN
HIGH ValveFNI
EndIf
IF KeyVal = KeyFNE THEN
HIGH ValveFNE
EndIf
IF KeyVal = KeyFOI THEN
HIGH ValveFOI
EndIf
IF KeyVal = KeyFOE THEN
HIGH ValveFOE
EndIf
IF KeyVal = KeyRNI THEN
HIGH ValveRNI
EndIf
IF KeyVal = KeyRNE THEN
HIGH ValveRNE
EndIf
IF KeyVal = KeyROI THEN
HIGH ValveROI
EndIf
IF KeyVal = KeyROE THEN
HIGH ValveROE
EndIf

Pause 100
GOSUB CloseAllValves
EndIf

RETURN

'*******************************************************
'*******************************************************

UpdateScreen2:
'< Lean Lean* >
'^ Pitch Pitch v
'UP All All DN
'\ Artic Artic /

'There is little info to update on the fly, so this screen is easy

IF KeyVal = 0 THEN
'Turn All valves off
GOSUB CloseAllValves
ELSE
'First update display based upon the button pressed, then open the valve

IF KeyVal = KeyFNI THEN 'Lean to Near
HIGH ValveFOI
HIGH ValveROI
HIGH ValveFNE
HIGH ValveRNE
EndIf

IF KeyVal = KeyFNE THEN 'Lean to Front
HIGH ValveFNE
HIGH ValveFOE
HIGH ValveRNI
HIGH ValveROI
EndIf

IF KeyVal = KeyRNI THEN 'All Up
HIGH ValveFNI
HIGH ValveFOI
HIGH ValveRNI
HIGH ValveROI
EndIf

IF KeyVal = KeyRNE THEN 'Artic \
HIGH ValveFNE
HIGH ValveFOI
HIGH ValveRNI
HIGH ValveROE
EndIf

IF KeyVal = KeyFOI THEN 'Lean Off
HIGH ValveFNI
HIGH ValveFOE
HIGH ValveRNI
HIGH ValveROE
EndIf

IF KeyVal = KeyFOE THEN 'Lean Back
HIGH ValveFNI
HIGH ValveFOI
HIGH ValveRNE
HIGH ValveROE
EndIf

IF KeyVal = KeyROI THEN 'All Down
HIGH ValveFNE
HIGH ValveFOE
HIGH ValveRNE
HIGH ValveROE
EndIf

IF KeyVal = KeyROE THEN 'Artic /
HIGH ValveFNI
HIGH ValveFOE
HIGH ValveRNE
HIGH ValveROI
EndIf

Pause 200
EndIf

RETURN

'*******************************************************
'*******************************************************

UpdateScreen3:
'This gives the highest degree of automatic control and is mostly intended for use on the road
'
'OnRd 888 777 OfRd
'^ Height 999 v
'000 ## 000 'These buttons trim the lean
'000 ## 000 'and these the
'
'000 is the pressure and # represents the current tilt sensor reading
'Trim is used to lean the vehicle from side to side
'^ & v control the ride height in fairly course steps
'Other buttons do nothing

PDisp = 0

'==========Begin self levelling!============
'Demand pressures are set by
'PressureFN
'PressureFO
'PressureRN
'PressureRO


'Read Pushbuttons to change height and tilt

'Use MinFN as representative of all four min pressures
IF KeyVal = KeyFNE THEN
SetHeight = SetHeight + 5
If SetHeight > (MinFN + 100) THEN
SetHeight = (MinFN + 100)
EndIf
EndIf

'Use MinFN as representative of all four min pressures
IF KeyVal = KeyFOE THEN
SetHeight = SetHeight - 5
If SetHeight < MinFN THEN
SetHeight = MinFN
EndIf
EndIf

'Trim left & right
IF KeyVal = KeyRNI THEN
SetTilt = SetTilt + 1
If SetTilt > 90 THEN
SetTilt = 90
EndIf
EndIf

IF KeyVal = KeyROI THEN
SetTilt = SetTilt - 1
If SetTilt < -90 THEN
SetTilt = -90
EndIf
EndIf

'Trim fore & aft
IF KeyVal = KeyRNE THEN
SetTiltFR = SetTiltFR + 1
If SetTiltFR > 90 THEN
SetTiltFR = 90
EndIf
EndIf

IF KeyVal = KeyROE THEN
SetTiltFR = SetTiltFR - 1
If SetTiltFR < -90 THEN
SetTiltFR = -90
EndIf
EndIf

'These two keys (bottom right) change between on and off road
'Off road, the tilt gain is increased and the hysteresis reduced
'It also shortens the time between updates to increase the response speed.
IF KeyVal = KeyFOI THEN
OffRoad = OffRoad + 1

'0 = Min air use
'1 = On road self level
'2 = Off Road
'3 = X Off road
'4 = XX Off Road

If OffRoad = 0 Then
'Normal on road - minimum air usage. No levelling, slow response
Hysteresis = 15
TiltGain = 50 'Remove any practical lean compensation
PulseLen = 100
ORLevelGate = 100 'No self level
ElseIf OffRoad = 1
'Normal on road - Full levelling, slow response
Hysteresis = 13
TiltGain = 10
PulseLen = 150
ORLevelGate = 0 'self level
ElseIf OffRoad = 2
'Normal off road - slow EQ with a bit of levelling
Hysteresis = 9
TiltGain = 6
PulseLen = 200
ORLevelGate = 10 'dont level unless > 10 degree lean
ElseIf OffRoad = 3
'Xtreme off road - Fast WQ
Hysteresis = 6
TiltGain = 3
PulseLen = 250
ORLevelGate = 20 'Dont level unless > 20 degree lean
ElseIf OffRoad > 3
'XXTreme off road - Fast level
OffRoad = 4
Hysteresis = 6
TiltGain = 1
PulseLen = 100 'We are going to add in lean to bump this up to, up to 400
ORLevelGate = 0 'Full self level
EndIf
EndIf

IF KeyVal = KeyFNI THEN
OffRoad = 0
Hysteresis = 9
TiltGain = 50 'Remove any practical lean compensation
PulseLen = 100
ORLevelGate = 100
EndIf



GOSUB GetPressures:
GOSUB GetTiltSensors

MeanHeight = (ANPressureFN + ANPressureFO + ANPressureRN + ANPressureRO)

'Display height
'Add in 'SetTiltFR' such that it is reflected on display
DiffFR = ((((ANPressureFN + ANPressureFO) / 2) - ((ANPressureRN + ANPressureRO) / 2)) - SetTiltFR) / 8
'Difference should have a range of 0 to 10

ColFR = DiffFR + 6
If ColFR > 12 then
ColFR = 12
Endif
IF ColFR < 0 then
ColFR = 0
Endif
ColLR = (Tilt) + 6
If ColLR > 12 Then
ColLR = 12
Endif
If ColLR < 0 Then
ColLR = 0
Endif

PosLR = 40 + ColLR + 3 + 64
PosFR = 60 + ColFR + 3 + 64

'0 = Min air use
'1 = On road self level
'2 = Off Road
'3 = X Off road
'4 = XX Off Road

'Print a block at line & col determined by lean/pitch
serout SerialOut,BAUD,[POSCMD,LastPosLR," "]
If OffRoad = 0 Then 'Display different block for off rd mode
serout SerialOut,BAUD,[POSCMD,PosLR,"=="] 'Min Air
ElseIf OffRoad = 1
serout SerialOut,BAUD,[POSCMD,PosLR,255,"L"] 'On Road Levelling
ElseIF OffRoad = 2 'Display OO
serout SerialOut,BAUD,[POSCMD,PosLR,"OG"] 'Gated Level
ElseIF OffRoad = 3 'Display XO
serout SerialOut,BAUD,[POSCMD,PosLR,"XG"] 'Gated Level
ElseIF OffRoad = 4 'Display XX
serout SerialOut,BAUD,[POSCMD,PosLR,"XL"] 'Full Level
EndIf

serout SerialOut,BAUD,[POSCMD,LastPosFR," "]
serout SerialOut,BAUD,[POSCMD,PosFR,255,255]

LastPosLR = PosLR
LastPosFR = PosFR


'Set Height with trim fore/aft
PressureFN = SetHeight + SetTiltFR
PressureRN = SetHeight - SetTiltFR
PressureFO = SetHeight + SetTiltFR
PressureRO = SetHeight - SetTiltFR


'Calculate tilt
PressureFN = PressureFN - AutoTilt
PressureRN = PressureRN - AutoTilt
PressureFO = PressureFO + AutoTilt
PressureRO = PressureRO + AutoTilt

'If we have already reached max articulation, try applying opposite to other side
IF PressureFN < MinFN Then
PressureFO = PressureFO + MinFN - PressureFN
PressureFN = MinFN
EndIf
IF PressureFO < MinFO Then
PressureFN = PressureFN + MinFO - PressureFO
PressureFO = MinFO
EndIf
IF PressureRN < MinRN Then
PressureRO = PressureRO + MinRN - PressureRN
PressureRN = MinRN
EndIf
IF PressureRO < MinRO Then
PressureRN = PressureRN + MinRO - PressureRO
PressureRO = MinRO
EndIf

IF PressureFN > PressureMax Then
PressureFO = PressureFO - PressureFN + PressureMax
PressureFN = PressureMax
EndIf
IF PressureFO > PressureMax Then
PressureFN = PressureFN - PressureFO + PressureMax
PressureFO = PressureMax
EndIf
IF PressureRN > PressureMax Then
PressureRO = PressureRO - PressureRN + PressureMax
PressureRN = PressureMax
EndIf
IF PressureRO > PressureMax Then
PressureRN = PressureRN - PressureRO + PressureMax
PressureRO = PressureMax
EndIf


GOSUB SetMaxMinPressures

'Set pressures in air bags
nCnt = nCnt + 1

'0 = Min air use
'1 = On road self level
'2 = Off Road
'3 = X Off road
'4 = XX Off Road
'React more quickly off road
If ((nCnt > 4) AND (OffRoad = 2)) OR ((nCnt > 2) AND (OffRoad = 3)) OR ((nCnt > 2) AND (OffRoad = 4)) OR ((nCnt > 8) AND (OffRoad <= 1)) Then
GOSUB SetPressuresByLookup
nCnt = 0
Else
GOSUB GetPressures:
GOSUB GetTiltSensors
GOSUB GetPressures:
GOSUB GetTiltSensors
EndIf

'Display preset lean & height
ColFR = (110 - SetHeight) / 10
If ColFR > 14 then
ColFR = 14
Endif
IF ColFR < 0 then
ColFR = 0
Endif
ColLR = 7 - (SetTilt /4 )
If ColLR > 14 Then
ColLR = 14
Endif
If ColLR < 0 Then
ColLR = 0
Endif

PosLR = 0 + ColLR + 2 + 64
PosFR = 20 + ColFR + 4 + 64


'Write Lean L/R
serout SerialOut,BAUD,[POSCMD,68," ",SDEC SetTilt," "]
'Write Lean F/R
serout SerialOut,BAUD,[POSCMD,73," ",SDEC SetTiltFR," "]

serout SerialOut,BAUD,[POSCMD,LastSetHeight," "]
serout SerialOut,BAUD,[POSCMD,PosFR,DEC SetHeight]

LastSetTilt = PosLR
LastSetHeight = PosFR

Return

'*******************************************************
'*******************************************************

GetKeyboardData:
' Now use Serin to gather input from 6-144 and convert
' it to a decimal number. Note that we are not checking
' for keypresses--"Serin 1\2.." automatically holds
' pin P2 low to tell 6-144 to send keypress data
' whenever it's available.

if KeyStrobe = 0 then
'serin SerialIn\KeyStrobe,BAUD,[KeyVal] ' Get key value
serin SerialIn,BAUD,[KeyVal] ' Get key value
'DEBUG [DEC KeyVal,10,13]
serout SerialOut,BAUD,[7] 'Make beep
serout SerialOut,BAUD,[4] 'Cursor off

Else
KeyVal = 0 'Equivalent to no key pressed
EndIf

If KeyVal = KeyNext THEN
ScreenID = ScreenID + 1
IF ScreenID = 4 THEN
ScreenID = 1
ENDIF
GOSUB DrawScreen 'Draw static screen elements
EndIF
If KeyVal = KeyPrev THEN
ScreenID = ScreenID - 1
If ScreenID = 0 THEN
ScreenID = 3
ENDIF
GOSUB DrawScreen 'Draw static screen elements
EndIF

RETURN



'*******************************************************
'*******************************************************

CloseAllValves:
'Simply resets the state
LOW ValveRNI
LOW ValveRNE
LOW ValveROI
LOW ValveROE

LOW ValveFNI
LOW ValveFNE
LOW ValveFOI
LOW ValveFOE
RETURN

'*******************************************************
'*******************************************************

GetPressures:
'This sets variables relating to all the pressure sensors
'Measure Analog values


MeanCnt = MeanCnt + 1
IF MeanCnt = 10 THEN
MeanCnt = 0
PressureValid = 1 'do not use readings until valid
EndIf

ADIN AX0,2,AD_RON,ANPressureFO
ADIN AX1,2,AD_RON,ANPressureFN
ADIN AX2,2,AD_RON,ANPressureRN
ADIN AX3,2,AD_RON,ANPressureRO

PrFN(MeanCnt) = ANPressureFN
PrFO(MeanCnt) = ANPressureFO
PrRN(MeanCnt) = ANPressureRN
PrRO(MeanCnt) = ANPressureRO


ANPressureFN = (PrFN(0) + PrFN(1) + PrFN(2) + PrFN(3) + PrFN(4) + PrFN(5) + PrFN(6) + PrFN(7) + PrFN(8) + PrFN(9)) / 10 '- 255
ANPressureFO = (PrFO(0) + PrFO(1) + PrFO(2) + PrFO(3) + PrFO(4) + PrFO(5) + PrFO(6) + PrFO(7) + PrFO(8) + PrFO(9)) / 10 '- 255
ANPressureRN = (PrRN(0) + PrRN(1) + PrRN(2) + PrRN(3) + PrRN(4) + PrRN(5) + PrRN(6) + PrRN(7) + PrRN(8) + PrRN(9)) / 10 '- 255
ANPressureRO = (PrRO(0) + PrRO(1) + PrRO(2) + PrRO(3) + PrRO(4) + PrRO(5) + PrRO(6) + PrRO(7) + PrRO(8) + PrRO(9)) / 10 '- 255

'ANPressureFN = ((ANPressureFN - 300))
'ANPressureFO = ((ANPressureFO - 300))
'ANPressureRN = ((ANPressureRN - 300))
'ANPressureRO = ((ANPressureRO - 300))

'IF ANPressureFN < 0 THEN
' ANPressureFN = 0
'ENDIF
'IF ANPressureFO < 0 THEN
' ANPressureFO = 0
'ENDIF
'IF ANPressureRN < 0 THEN
' ANPressureRN = 0
'ENDIF
'IF ANPressureRO < 0 THEN
' ANPressureRO = 0
'ENDIF

'When we are setting up scale and zero, we need to know pressures without their effect
If IgnoreScale = 0 THEN
ANPressureFN = ((ANPressureFN - (SetZeroFN * 10)) * 10 / SetScaleFN)
ANPressureFO = ((ANPressureFO - (SetZeroFO * 10)) * 10 / SetScaleFO)
ANPressureRN = ((ANPressureRN - (SetZeroRN * 10)) * 10 / SetScaleRN)
ANPressureRO = ((ANPressureRO - (SetZeroRO * 10)) * 10 / SetScaleRO)
EndIf

' Debug [dec ANPressureFO, " ", dec ANPressureFN, " ", dec ANPressureRN, " ", dec ANPressureRO, 10,13]

Return

'*******************************************************
'*******************************************************

GetTiltSensors:
'Tilt going +ve means we need to lean to off side

TiltCnt = TiltCnt + 1
If TiltCnt > 9 Then
TiltCnt = 0
EndIf

PULSIN TiltSensor1,1,Tilt

'Calcilate 10 point moving time average for Tilt
TiltMean(TiltCnt) = Tilt
Tilt = (TiltMean(0) + TiltMean(1) + TiltMean(2) + TiltMean(3) + TiltMean(4) + TiltMean(5) + TiltMean(6) + TiltMean(7) + TiltMean(8) + TiltMean(9)) / 10 '- 255

'Convert such that horizontal is zero
Tilt = Tilt - 1600

AutoTilt = ((Tilt + (SetTilt * 7)) / TiltGain)
Tilt = ((Tilt + (SetTilt * 7)) / 14)

If OffRoad = 4 Then
'XXTreme off road
If ABS(Tilt) > 7 Then
PulseLen = 400
ElseIf ABS(Tilt) > 3
PulseLen = 200
Else
PulseLen = 100
EndIf
EndIf

'In some of the off road modes, concentrate on equal pressures rather than levelling
If ABS(AutoTilt) < ORLevelGate Then
AutoTilt = 0
EndIf

If OffRoad < 2 Then
If AutoTilt < -30 Then
AutoTilt = -30
EndIf
If AutoTilt > 30 Then
AutoTilt = 30
EndIf
EndIf

RETURN

'*******************************************************
'*******************************************************

SetPortStates:
'Set the input/output state of all the ports


OUTPUT ValveRNI
LOW ValveRNI
OUTPUT ValveRNE
LOW ValveRNE
OUTPUT ValveROI
LOW ValveROI
OUTPUT ValveROE
LOW ValveROE

OUTPUT ValveFNI
LOW ValveFNI
OUTPUT ValveFNE
LOW ValveFNE
OUTPUT ValveFOI
LOW ValveFOI
OUTPUT ValveFOE
LOW ValveFOE

'Enable Pull Up resistors on Pins 0 to 7
'SetPullUps PU_OFF 'PU_ON

GOSUB CloseAllValves

INPUT TiltSensor1 'Tilt Switch
INPUT TiltSensor2 'Tilt Switch

INPUT KeyStrobe

Return

'*******************************************************
'*******************************************************

SetPressuresByLookup:
'This is a simple successive approximation.
'It calculates which spring needs the biggest change
'in pressure and does that one only

DiffFN = ABS(PressureFN - ANPressureFN)
DiffFO = ABS(PressureFO - ANPressureFO)
DiffRN = ABS(PressureRN - ANPressureRN)
DiffRO = ABS(PressureRO - ANPressureRO)

serout SerialOut,BAUD,[POSCMD,104,DEC DiffFN, " "]
serout SerialOut,BAUD,[POSCMD,121,DEC DiffFO, " "]
serout SerialOut,BAUD,[POSCMD,124,DEC DiffRN, " "]
serout SerialOut,BAUD,[POSCMD,141,DEC DiffRO, " "]

If (DiffFN > DiffFO) AND (DiffFN > DiffRN) AND (DiffFN > DiffRO) Then
'FN needs changing
IF DiffFN < Hysteresis THEN
RETURN 'Only change if change big enough
EndIf
IF (PressureFN - ANPressureFN) > 0 THEN
'If Compressor = 0 THEN 'Make sure reservoir is full
HIGH ValveFNI
'EndIf
Else
HIGH ValveFNE
EndIf
EndIf

If (DiffFO > DiffFN) AND (DiffFO > DiffRN) AND (DiffFO > DiffRO) Then
'FO needs changing
IF DiffFO < Hysteresis THEN
RETURN 'Only change if change big enough
EndIf
IF (PressureFO - ANPressureFO) > 0 THEN
'If Compressor = 0 THEN 'Make sure reservoir is full
HIGH ValveFOI
'EndIf
Else
HIGH ValveFOE
EndIf
EndIf

If (DiffRN > DiffFO) AND (DiffRN > DiffFN) AND (DiffRN > DiffRO) Then
'RN needs changing
IF DiffRN < Hysteresis THEN
RETURN 'Only change if change big enough
EndIf
IF (PressureRN - ANPressureRN) > 0 THEN
'If Compressor = 0 THEN 'Make sure reservoir is full
HIGH ValveRNI
'EndIf
Else
HIGH ValveRNE
EndIf
EndIf

If (DiffRO > DiffFO) AND (DiffRO > DiffFN) AND (DiffRO > DiffRN) Then
'RO needs changing
IF DiffRO < Hysteresis THEN
RETURN 'Only change if change big enough
EndIf
IF (PressureRO - ANPressureRO) > 0 THEN
'If Compressor = 0 THEN 'Make sure reservoir is full
HIGH ValveROI
'EndIf
Else
HIGH ValveROE
EndIf
EndIf

Pause PulseLen

GOSUB CloseAllValves


RETURN

'*******************************************************
'*******************************************************

SetMaxMinPressures:
'Make sure demand pressures are within range

'Dont worry about Max or Min pressures for Xtreme off road
if OffRoad < 2 then
If PressureFN > PressureMax Then
PressureFN = PressureMax
ENDIF
If PressureFN < MinFN Then
PressureFN = MinFN
ENDIF

If PressureFO > PressureMax Then
PressureFO = PressureMax
ENDIF
If PressureFO < MinFO Then
PressureFO = MinFO
ENDIF

If PressureRN > PressureMax Then
PressureRN = PressureMax
ENDIF
If PressureRN < MinRN Then
PressureRN = MinRN
ENDIF

If PressureRO > PressureMax Then
PressureRO = PressureMax
ENDIF
If PressureRO < MinRO Then
PressureRO = MinRO
ENDIF
EndIf
RETURN

'*******************************************************
'*******************************************************

ZeroVars:
FOR nCnt = 0 to 9
PrFN(nCnt) = 0
PrFO(nCnt) = 0
PrRN(nCnt) = 0
PrRO(nCnt) = 0
TiltMean(nCnt) = 0
NEXT
FOR nCnt = 0 to 9
GOSUB GetPressures
GOSUB GetTiltSensors
NEXT

'Min pressures for each bag
MinFN = 10
MinFO = 10
MinRN = 10
MinRO = 10

IgnoreScale = 0

'Start out in On Road mode
OffRoad = 0

RETURN