This code is being continually tweaked. Compass code will be added soon.
' {$STAMP BS2}
' {$PBASIC 2.5}
' Underwater ROV Stuart Weightman 2007
'*** A/D setup
CS CON 3 ' 0831 chip select active low from BS2 (P3)
CLK CON 4 ' Clock pulse from BS2 (P4) to 0831
Dout CON 5 ' Serial data output from 0831 to BS2 (P5)
Depth VAR Byte ' Variable to hold incoming number (0 to 255)
'*** Motor outputs
M3 CON 13 : M2 CON 14 : M1 CON 15
'*** Variables
Speed VAR Word : Turn VAR Word : Vertical VAR Word :DepthSP VAR Word :Auto VAR Bit : Man VAR Bit
Zero VAR Bit : Allstop VAR Bit : ES VAR Bit : X VAR Byte : BumplessTnf VAR Word
Ascend VAR Bit : Descend VAR Bit : L VAR Byte : Bias VAR Byte : T VAR Word
'*** Variable initialise
Speed = 0 : Turn = 0 : Vertical = 750 : L = 0 : Bias = 0 : T = 0
'*** Controller variables
Kp VAR Word
Current CON 0 ' Array index for current error
Accumulator CON 1 ' Array index for accumulated error
Error VAR Word(2)
I VAR Word ' Integral term
Offset CON 750 ' Offset of 750 for servo / MD2 control
T2 VAR Byte
T2 = 0 ' Timer
Kp = 1 ' Gain change this one ****************
IAT CON 10 ' IAT change this one ****************
GOSUB StopMotors 'Stop all motors on programme initialisation
SEROUT 16,84,["!O Man=1",CR]'Select/tick manual control on start up
Main1:
T = T + 1 : IF T > 10 THEN GOSUB Main2'Timer to service lower priority code less frequently
GOSUB DepthData 'Get depth
GOSUB Plot 'Manipulate and plot
SEROUT 16,84,["!READ (ES)",CR]
SERIN 16,84,1000,Main1,[SDEC ES]
PAUSE 100
IF ES = 1 THEN GOSUB EmergencySurface 'Emergency surface
SEROUT 16,84,["!READ (ALLSTOP)",CR]
SERIN 16,84,1000,Main1,[SDEC ALLSTOP]
PAUSE 100
IF ALLSTOP = 1 THEN GOSUB StopMotors
SEROUT 16,84,["!READ (AUTO)",CR] 'Read Auto depth control button
SERIN 16,84,1000,Main1,[SDEC AUTO]
PAUSE 100
IF AUTO = 1 THEN GOSUB AutoControl
SEROUT 16,84,["!READ (MAN)",CR] 'Read Manual operation control button
SERIN 16,84,1000,Main1,[SDEC MAN]
PAUSE 100
IF MAN = 1 THEN GOSUB ManControl
DEBUG "!READ (SPEED)",CR 'Read StampPlot GUI object using DEBUG
SERIN 16,84,1000,Main1,[SDEC SPEED] 'Accept returning data -250 to +250
PAUSE 100 'Pause for comms
SPEED = SPEED + 750 'Add 750 to rescale 550 to 950 for MD22 motor board
PULSOUT 14, SPEED 'which means 750 is mid Stopped position
SEROUT 16,84,["!READ (TURN)",CR] 'Read StampPlot GUI object using SEROUT
SERIN 16,84,1000,Main1,[SDEC TURN] 'Accept returning data
PAUSE 100
TURN = TURN + 750
PULSOUT 15, TURN
GOTO Main1
Main2:
T = 0
GOSUB CheckForLeak
SEROUT 16,84,["!READ (ZERO)",CR] 'Read Zero depth button
SERIN 16,84,1000,Main1,[SDEC ZERO]
PAUSE 100
IF ZERO = 1 THEN GOSUB ZeroDepth
RETURN
'******** Subroutines **********
AutoControl:
SEROUT 16,84,["!READ (DEPTHSP)",CR] 'Read Auto control depth setpoint slider
SERIN 16,84,1000,Main1,[DEC DEPTHSP]
PAUSE 100
DepthSP = 100 - DepthSP 'Invert DepthSP for plot,(upside down plot scale)
DEBUG "!O plot1.draw=ACHN 2,", DEC DepthSP ,",(Red)",CR 'Draw command required for plotting Obj plot1
'with more than 1 analogue value
T2 = T2 + 1
IF T2 > IAT THEN Integral
RETURN
Integral:
T2=0
Error(Current) = DepthSP - Depth
Error(Accumulator) = Error(Accumulator) + Error(Current)
I = Kp * Error(Accumulator)
Vertical = I + Offset + BumplessTnf MIN 550 MAX 950
BumplessTnf = 0
GOSUB M3Thruster
RETURN
ManControl:
SEROUT 16,84,["!O AUTO=0",CR] 'Untick Auto button
SEROUT 16,84,["!READ (ASCEND)",CR]
SERIN 16,84,1000,Main1,[SDEC ASCEND]
PAUSE 100
IF ASCEND = 1 THEN GOSUB Up
SEROUT 16,84,["!READ (DESCEND)",CR]
SERIN 16,84,1000,Main1,[SDEC DESCEND]
PAUSE 100
IF DESCEND = 1 THEN GOSUB Down
Depth = 100 - Depth
SEROUT 16,84,["!O DepthSP =",DEC Depth,CR] 'Update auto SP slider to actual depth
BumplessTnf = Vertical - Offset
Error(Accumulator) = 0
RETURN
EmergencySurface:
Vertical = 900 'Vertcal motor 850 speed to ascend
GOSUB M3Thruster
SEROUT 16,84,["!O AUTO=0",CR] 'Untick Auto depth control button
GOSUB DepthData
GOSUB Plot
IF Depth < 20 THEN GOSUB StopMotors
IF Depth < 20THEN SEROUT 16,84,["!O ES = 0",CR] 'Untick ES button
RETURN
DepthData: 'Acquire conversion from A/D 0831
LOW CS 'Select the chip
LOW CLK 'Ready the clock line.
PULSOUT CLK,10 'Send a 10 uS clock pulse to the 0831
SHIFTIN Dout, CLK, MSBPOST,[Depth\8] 'Shift in data
HIGH CS 'Stop conversion
RETURN
Plot:
Depth = Depth - Bias 'Incorporate Zeroing bias
SEROUT 16,84,["!O ROVDepthMeter =",DEC Depth,CR] 'Update meter with depth
Depth = 100 - Depth 'Invert depth for plot, (upside down scale)ie. a reading of 2 now = 98
DEBUG "!O PLOT1=",DEC Depth,CR 'Manually update plot1 with depth
RETURN
Zerodepth:
GOSUB DepthData
Bias = Depth - 2 'Manipulate data ie. force the depth indication on plot to +2
SEROUT 16,84,["!O Zero=0",CR] 'Untick Zero button
RETURN
Up:
Vertical = Vertical + 15
GOSUB M3Thruster
IF Vertical > 950 THEN Vertical = 950
SEROUT 16,84,["!O Ascend=0",CR] 'Untick Ascend button
RETURN
Down:
Vertical = Vertical - 15
GOSUB M3Thruster
IF Vertical < 550 THEN Vertical = 550
SEROUT 16,84,["!O Descend=0",CR] 'Untick Descend button
RETURN
CheckForLeak:
IF IN0 = 0 THEN Leak
RETURN
Leak:
SEROUT 16,84,["!O LeakMeter =",DEC 100,CR] 'Update leak meter with a high value
DEBUG "~IWAV nralarm.wav",CR
PAUSE 800
SEROUT 16,84,["!O LeakMeter =",DEC 0,CR] 'Update leak meter with a low value
GOTO Main2
StopMotors:
X = 0
DO WHILE X < 5 'Send stop 5 times to be sure
X = X + 1
SEROUT 16,84,["!O SPEED = 0",CR] 'Move slider to centre OFF position
SEROUT 16,84,["!O TURN = 0",CR] 'Move slider to centre OFF position
PULSOUT M1, 750 'Set Left Motor servo o/p 15 to stop, mid position
PULSOUT M2, 750 'Set Right Motor servo o/p 14 to stop, mid position
Vertical = 750 'Set Vertical Motor servo o/p 13 to stop, mid position
GOSUB M3Thruster
LOOP
RETURN
M3Thruster:
PULSOUT M3, Vertical
SEROUT 16,84,["!O VerticalSpeed =",DEC Vertical,CR] 'Update GUI with VerticalSpeed indication
RETURN