To get a better idea of the specifics of what I need, here is Basic Stamp code for a 5 turret destroyer. My friends and I couldn't get it to work properly...
' {$STAMP BS2sx}
' {$PBASIC 2.5}
' The two lines above identify the Basic Stamp type and PBASIC version,
' used in this program.
'==========================================
'= Gun Fire Control System =
'==========================================
'
' Author: Nico Ottevaere
' Filename: DD_GFCS_21_with_comments.BSX
' Revision: 2.1
' Date: 10 Nov 04
'
' This program allows control of the main gun battery of a 5-gun destroyer
' to be controlled by 2 RC channels.
' One channel controls the gun director bearing
' Another channel controls the mode of operation
' OFF : Director and guns align with ships centerline
' STANDBY : De guns remain in their last controlled position,
' the director is controlled by the RC bearing channel
' ASSIGNED : The guns follow the director, when within rotation angle.
' Otherwise they train to the closest possible position.
'
' Program Revision History:
' 1.0 Proportional position control of target, 6 channels
' 1.1 Introduced 4 control groups instead of 6 channels to speed up program
' 2.0 Proportional speed control of target bearing
' 2.1 Incorporated separate maximum rotating speed for director or guns
' PDL
' Initialize system
' Main Loop:
' Adjust servo position but limit training rate
' Send servo pulses
' Change the status LED indication
' Measure bearing information pulse from receiver
' Adjust targets position for measured pulse (speed-control)
' Measure mode information pulse from receiver
' Decode mode
' Mode = OFF
' Make all Desired Positions for all CGs centerline
' Continue at Main Loop
' Mode = STANDBY
' Make Desired Position for CG1 (gun director) equal to Target
' Continue at Main Loop
' Mode = ASSIGNED
' Make Desired Position for CG1 (gun director) equal to Target
' Make Desired Position for CG2 (Turrets #1 & #2) equal to Target
' IF Target within range of turret #3
' Make Desired Position for CG3 (Turret #3) equal to Target
' ELSE
' Set Desired Position for CG3 (Turret #3) as close as possible
' to Target
' IF Target within range of turrets #4 & #5
' Calculate a Desired Position for CG4 (Turret #4 & #5) so
' that the guns align with Target
' ELSE
' Set Desired Position for CG4 (Turret #4 & #5) as close
' as possible to Target
' Continue at Main Loop
'==========================================
'= DEFINITIONS =
'==========================================
'
'------------------------------------------
'- Constants -
'------------------------------------------
'
MPX_Mid CON 1875 ' Duration of RC pulse when at center position
' BS-SX has pulse measuring resolution of 0.8us
' 1875 * 0.8 us totals 1500us or 1.5ms
Min_Pulse CON 680 ' Minimum pulse length that can be applied
' to Parallax 180DEG servo.
' 680 * 0.8us = 480us or 0.48ms
Max_Pulse CON 2810 ' Maximum pulse length that can be applied
' to Parallax 180DEG servo.
' 2810 * 0.8us = 2248us or 2.25ms
' Pulse value has a range of 2810 - 680 =
' 2130. This corresponds to 270DEG of
' rotation of the turrets so each degree of
' rotation corresponds to app. 8 units
Mid_Pulse CON 1745 ' Mid pulse to put Parallax 180DEG servos
' centerline
' this is not the mathematic center of the pulse
' range because of the non-linear operation due
' to extreme pulses
' Response is most linear around mid pulse
P3_Limit CON 1600 ' Port training limit of #3 gun
SB3_Limit CON 1890 ' Starboard training limit of #3 gun
P4_Limit CON 1550 ' Port training limit of #4 and #5 gun
SB4_Limit CON 2030 ' Starboard training limit of #4 and #5 gun
' Following are 180DEG correction values
' for guns #4 and #5
Offset_P CON 1400 ' Offset positive
Offset_M CON 1400 ' Offset minus
' I have two values to be able to make final
' adjustments when installed and operational
Turret_Rate CON 15 ' Maximum rate at which turret can train
' 15 units is about 2DEG, calculations are done
' 50 times a second so the maximum training rate
' is 100DEG / second
Director_Rate CON 25 ' Maximum rate at which director can train
' 25 units is about 3DEG, calculations are done
' 50 times a second so the maximum training rate
' is 150DEG / second
'------------------------------------------
'- Input Output configuration -
'------------------------------------------
'
Alive PIN 7 ' Assign name "Alive" to IO-pin 7
Receiver_Bearing PIN 15 ' Assign name "Receiver_Bearing" to IO-pin 15
Receiver_Mode PIN 14 ' Assign name "Receiver_Mode" to IO-pin 14
' The servo pulses are generated by the command
' PULSOUT. This command sets the IO configuration
' of concerned PIN each time it executes
'------------------------------------------
'- Variables -
'------------------------------------------
'
Mode VAR Nib ' Nibble sized variable (4bits) that holds
' the current operating mode
' 0 = OFF
' 1 = STANDBY
' 2 = ASSIGNED
Index VAR Nib ' Nibble sized variable used as a counter (0 - 15)
Pulse_In VAR Word ' Word sized variable (16bits) to hold measured
' pulse length values
Target VAR Word ' Word sized variable that holds the bearing
' value of the target in "pulse length format" so
' 1745 means dead ahead
Current_Pos VAR Word(4) ' Array of 4 words, one for each Control Group,
' holding the current pulse length value
' Current_Pos(0) for the Gun Director
' Current_Pos(1) for the turrets #1 and #2
' Current_Pos(2) for turret #3
' Current_Pos(3) for the turrets #4 and #5
Desired_Pos VAR Word(4) ' Array of 4 words, one for each Control Group,
' holding the desired pulse length value
' Similar to Current_Pos(0-3) here above
Error VAR Word ' Word sized variable used in calculations
'==========================================
'= PROGRAM =
'==========================================
'-----------------------------------------
'-- Initialization --
'-----------------------------------------
' Set all turrets and gun director in centerline position.
' Set mode to OFF and Target to dead ahead
Initialize: ' Label to make code more readable
FOR Index = 0 TO 3 ' This FOR NEXT loop starts with Index = 0
' so the first pass, Current_Pos(0) and
' Desired_Pos(0)are set to centerline
Current_Pos(Index) = Mid_Pulse
Desired_Pos(Index) = Mid_Pulse
NEXT ' Here Index is incremented with 1 and if the
' condition is still true, the loop is performed
' again. So Index now gets the value 1, then 2,
' then 3.
' After the fourth pass Index would be 4 but this
' doesn't comply with the condition so no more
' loops are executed
Mode = 0 ' Mode is set to "OFF"
Target = Mid_Pulse ' Target is set to be dead ahead
'-----------------------------------------
'-- Main Loop --
'-----------------------------------------
Main_Loop: ' Label to make code more readable
' A label is also used to direct the program to
' a certain entry point
GOSUB Slew_to_Bearing ' Hold execution of this program and go execute a
' subroutine starting at the label
' "Slew_to_Bearing:"
' Upon completion of this subroutine, continue
' with the next line
GOSUB Send_Pulses ' Again go execute a subroutine this time
' labeled "Send_Pulses:", then continue next line
TOGGLE Alive ' Make IO Pin "Alive" (defined to be PIN 7) an
' OUTPUT and change its state. If 0 make 1,
' if 1 make 0.
' As the status LED is connected to this PIN,
' it blinks ON and OFF for each program pass
Measure_Receiver_Bearing_Pulse: ' Label to make code more readable
' Next instruction will configure PIN
' "Receiver_Bearing" as an input and wait for a
' positive pulse. If no pulse is detected before
' the command times out, it returns a 0
' Otherwise, the duration is counted in 0.8us
' for the SX version of the Basic Stamp
PULSIN Receiver_Bearing, 1, Pulse_In
' If the PULSIN instruction returned a 0, then
' no pulse was measured yet, so go to label
' "Measure_Receiver_Bearing_Pulse:" and measure
' again. In fact the program waits here until it
' gets the pulse and thus synchronizes with the
' RC receiver pulse sequence
IF ( Pulse_In = 0 ) THEN Measure_Receiver_Mode_Pulse
Adjust_Target_Bearing: ' Label to make code more readable
' Now we have to convert the measured pulse value
' into a usable value with witch we can adjust
' the targets bearing.
' Dividing the measured pulse value by 40 gives
' a value ranging from 32 to 66.
' By subtracting 49, the resulting range becomes
' -17 to +17 (speed value representing the
' sticks direction and displacement)
IF Mode > 0 THEN ' On the condition that mode is not OFF
Pulse_In = (Pulse_In / 40) - 49 ' Convert measured pulse
Target = Target - Pulse_In MIN Min_Pulse ' Adjust targets position by
' subtracting the calculated speed value BUT
' let Target not become smaller than Min_Pulse.
Target = Target MAX Max_Pulse ' If speed is negative, subtracted from target
' makes Target becomes bigger, so also make sure
' that Targets doesn't become bigger than
' Maximum pulse.
ENDIF ' End of this conditional section of code
Measure_Receiver_Mode_Pulse: ' Label to make code more readable
' Same variable is used again, but now to measure
' a positive pulse on PIN "Receiver_Mode"
PULSIN Receiver_Mode, 1, Pulse_In
IF ( Pulse_In = 0 ) THEN Measure_Receiver_Mode_Pulse
Determine_Mode: ' Label to make code more readable
' Now we have to decode the commanded mode.
' As this is done with a 3-position switch,
' the values can either be MIN, Mid or Max
' RC pulse value.
' As there is no trim on this RC function,
' I included a dead-zone around center position.
' Only the extremes will extend beyond the
' dead-zone
Mode = 1 ' Make mode STANDBY (as if a Mid pulse was received
IF Pulse_In > MPX_Mid + 300 THEN' If the measured value is larger
Mode = 2 ' then make mode = ASSIGNED
ELSEIF Pulse_In < MPX_Mid - 300 THEN ' else if measured value is smaller
Mode = 0 ' then make mode = OFF
ENDIF ' End of conditional section.
' If non of the conditions was met, then the mode
' remains STANDBY as set before the IF statement
BRANCH Mode, [Off, Standby, Assigned] ' This instruction will, depending on the
' value of Mode continue program execution as
' follows:
' Mode = 0 go to label "Off:"
' Mode = 1 go to label "Standby:"
' Mode = 2 go to label "Assigned:"
' +++++++++++++++++++++++
' ++ OFF ++
' +++++++++++++++++++++++
Off: ' Label to make code more readable
' Now come the specific things to do in OFF
FOR Index = 0 TO 3 ' Loop for all Control Groups to set the desired
' position to centerline.
Desired_Pos(Index) = Mid_Pulse
NEXT
GOTO Main_Loop ' Continue program execution at label "Main_Loop:"
' +++++++++++++++++++++++
' ++ STANDBY ++
' +++++++++++++++++++++++
Standby: ' Label to make code more readable
' In this mode, only the director is controlled
' Its desired position becomes the target, and
' as the target is controlled by the RC
' transmitter, one controls in fact the director
Desired_Pos(0) = Target
GOTO Main_Loop ' Nothing is changed on the turrets desired
' positions, so these do not move.
' Continue program execution at label "Main_Loop:"
' +++++++++++++++++++++++
' ++ Assigned ++
' +++++++++++++++++++++++
Assigned: ' Label to make code more readable
CG_Director:
Desired_Pos(0) = Target ' Align director with target
Guns_1_and_2:
Desired_Pos(1) = Target ' Align guns #1 and #2 with target
' In my system these have the same mechanical
' range and same direction for centerline so
' they rotate from -135DEG (Port)
' over 0DEG centerline forward TO +135DEG (STBD)
' I can always simply slave these guns to the
' director
CG_Gun_3: ' Now come the calculations for turret #3. It is
' also pointing forward, but has a sector around
' centerline in which it can not be used
' (do NOT fire into own ship)
' So only if the target is outside this sector
IF (Target < P3_Limit) OR (Target > SB3_Limit) THEN
Desired_Pos(2) = Target ' Align turret #3 with target
ELSE ' otherwise
IF Target > Mid_Pulse THEN ' If the Target is to starboard,
Desired_Pos(2) = SB3_Limit' then set the gun to the starboard sector limit
ELSE
Desired_Pos(2) = P3_Limit ' else set the gun to the port sector limit
ENDIF
ENDIF
CG_Guns_4_and_5: ' Calculations for guns 4 and 5 have also to
' consider that these guns have their center
' position facing aft These guns rotate from
' -45DEG (PORT) over 180DEG centerline AFT
' to +45DEG (STBD)
' In my setup the servo rotation is therefore
' reversed. These guns can not train in the
' sector -45 - 0 - +45. So first verify that the
' target is outside the no train zone
IF (Target < P4_Limit) OR (Target > SB4_Limit) THEN
IF Target > Mid_Pulse THEN ' Now if the target is on the starboard side
' make the desired position target - 180DEG, but
' let it not be less than Min pulse
Desired_Pos(3) = Target - Offset_M MIN Min_Pulse
ELSE ' else target on port side
' make the desired position target + 180DEG, but
' let it not be more than Max pulse
Desired_Pos(3) = Target + Offset_P MAX Max_Pulse
ENDIF
ELSE ' So the target is in a no train zone
IF Target => Mid_Pulse THEN ' If target is dead ahead or to starboard
' train the turrets max forward to port
Desired_Pos(3) = Min_Pulse
ELSE ' so target is to port
' train the turrets max forward to starboard
Desired_Pos(3) = Max_Pulse
ENDIF
ENDIF
GOTO Main_Loop ' Continue at label "Main_Loop:"
'==========================================
'= SUBROUTINES =
'==========================================
'------------------------------------------
'-- Send_Pulses --
'------------------------------------------
'
' Version 2.0
' 10 Nov 04
' Subroutine that generates the servo pulses to IO pins 0 - 3
' 0 : Gun Director
' 1 : Turrets 1 and 2
' 2 : Turret 3
' 3 : Turrets 4 and 5
' The value for the pulses is stored in the array Current_Pos
' INPUT : Current_Pos() Array of words holding pulse values
' OUTPUT : Servo Pulses to pins 0 through 3
' VARIABLES : Index
' Written: 28 Oct 04
' Modifications
' Version: 2 Reduced number of pulses from 6 to 4 to reduce
' duration of routine
' PDL
' Send servo pulses
' For each control group
' Send servo pulse of "current position" length
' End of subroutine
Send_pulses:
FOR Index = 0 TO 3 ' Loop for all Control Groups
PULSOUT Index, ( Current_Pos(Index) )' This command defines PIN(Index) as an
' OUTPUT and generates a pulse with a duration of
' Current_Position(Index) * 0.8 us (BS-SX)
NEXT ' Next Index
RETURN ' Return from this subroutine. The program now
' continues at the line after the call to this
' subroutine
'------------------------------------------
'-- Slew_to_Bearing --
'------------------------------------------
'
' Version 2.0
' 29 Nov 04
' Slew selected turret to target pulse and limit speed to Slew_Rate
' INPUT : Desired_Pos(Index)
' Index (Selected servo channel 0 = gun director
' 1 = turrets 1 and 2
' 2 = turret 3
' 3 = turrets 4 and 5
' Current_Pos(Index)
' OUTPUT : Current_Pos(Index) (Corrected position of selected servo channel)
' VARIABLES : Error, Current_Pos(Index)
' Written: 28 Oct 04
' Modifications
' Version: 2 added separate calculations for director to give it
' bigger speed than turrets
' PDL
' Director Calculation
' If current position >= desired position then go to Decrement_Director
' Error = current position - desired position, but limit to Director_Rate
' Add Error to current position
' Continue with gun calculations
' Decrement_Director
' Error = desired position - current position, but limit to Director_Rate
' Subtract Error from current position
' Gun Calculations
' For each gun control group
' If current position >= desired position then go to Decrement_Gun
' Error = current position - desired position, but limit to Turret_Rate
' Add Error to current position
' Continue with gun calculations
' Decrement_Gun
' Error = desired position - current position, but limit to Director_Rate
' Subtract Error from current position
' End of subroutine
Slew_to_Bearing:
Director:
IF (Current_Pos(0) >= Desired_Pos(0)) THEN Decrement_D
Error = Desired_Pos(0) - Current_Pos(0) MAX Director_Rate
Current_Pos(0) = Current_Pos(0) + Error
GOTO Guns
Decrement_D:
Error = Current_Pos(0) - Desired_Pos(0) MAX Director_Rate
Current_Pos(0) = Current_Pos(0) - Error
Guns:
FOR Index = 1 TO 3
IF (Current_Pos(Index) >= Desired_Pos(Index)) THEN Decrement_G
Error = Desired_Pos(Index) - Current_Pos(Index) MAX Turret_Rate
Current_Pos(Index) = Current_Pos(Index) + Error
GOTO Done_Slewing
Decrement_G:
Error = Current_Pos(Index) - Desired_Pos(Index) MAX Turret_Rate
Current_Pos(Index) = Current_Pos(Index) - Error
Done_slewing:
NEXT
RETURN
