' {$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.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 allign with ships centreline ' 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 withen 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 programme ' 2.0 Proportional speed control of target bearing ' 2.1 Incorporated separate maximum ratating speed for director or guns ' PDL ' Initialize system ' Main Loop: ' Adjut servo position but limit training rate ' Send servo pulses ' Change the status LED incication ' 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 centreline ' 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 (Turrets #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 Min_Pulse CON 680 ' Minimum pulse length Parallax 180DEG Max_Pulse CON 2810 ' Maximum pulse length Parallax 180DEG Mid_Pulse CON 1745 ' Mid pulse to put Parallax 180DEG servos centerline 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 Offset_P CON 1400 ' 180DEG Offset positive Offset_M CON 1400 ' 180DEG Offset minus Turret_Rate CON 15 ' Maximum rate at which turret can train Director_Rate CON 25 ' Maximum rate at which director can train '------------------------------------------ '- 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 '------------------------------------------ '- Variables - '------------------------------------------ ' Mode VAR Nib ' Nibble sized variablethat holds the current Mode Index VAR Nib ' Nibble sized variable used as a counter (0 - 15) Pulse_In VAR Word ' Word sized variable hold measured pulse lentgh ' values Target VAR Word ' Word sized variable that holds the bearing value of the ' of the target. Current_Pos VAR Word(4) ' Array of 4 words, one for each Control Group Desired_Pos VAR Word(4) ' Array of 4 words, one for each Control Group Error VAR Word ' Word sized variable used in calculations '========================================== '= PROGRAM = '========================================== '----------------------------------------- '-- Initialisation -- '----------------------------------------- Initialize: FOR Index = 0 TO 3 Current_Pos(Index) = Mid_Pulse Desired_Pos(Index) = Mid_Pulse NEXT Mode = 0 Target = Mid_Pulse '----------------------------------------- '-- Main Loop -- '----------------------------------------- Main_Loop: GOSUB Slew_to_Bearing GOSUB Send_Pulses TOGGLE Alive Measure_Receiver_Bearing_Pulse: PULSIN Receiver_Bearing, 1, Pulse_In IF ( Pulse_In = 0 ) THEN Measure_Receiver_Mode_Pulse Adjust_Target_bearing: IF Mode > 0 THEN Pulse_In = (Pulse_In / 40) - 49 Target = Target - Pulse_in MIN Min_Pulse Target = Target MAX Max_Pulse ENDIF Measure_Receiver_Mode_Pulse: PULSIN Receiver_Mode, 1, Pulse_In IF ( Pulse_In = 0 ) THEN Measure_Receiver_Mode_Pulse Determine_Mode: Mode = 1 IF Pulse_In > MPX_Mid + 300 THEN Mode = 2 ELSEIF Pulse_In < MPX_Mid - 300 THEN Mode = 0 ENDIF BRANCH Mode, [Off, Standby, Assigned] ' +++++++++++++++++++++++ ' ++ OFF ++ ' +++++++++++++++++++++++ Off: ' FOR Index = 0 TO 3 Desired_Pos(Index) = Mid_Pulse NEXT GOTO Main_Loop ' +++++++++++++++++++++++ ' ++ STANDBY ++ ' +++++++++++++++++++++++ Standby: Desired_Pos(0) = Target GOTO Main_Loop ' +++++++++++++++++++++++ ' ++ Assigned ++ ' +++++++++++++++++++++++ Assigned: Desired_Pos(0) = Target Desired_Pos(1) = Target IF (Target < P3_Limit) OR (Target > SB3_Limit) THEN Desired_Pos(2) = Target ELSE IF Target > Mid_Pulse THEN Desired_Pos(2) = SB3_Limit ELSE Desired_Pos(2) = P3_Limit ENDIF ENDIF IF (Target < P4_Limit) OR (Target > SB4_Limit) THEN IF Target > Mid_Pulse THEN Desired_Pos(3) = Target - Offset_M MIN Min_Pulse ELSE Desired_Pos(3) = Target + Offset_P MAX Max_Pulse ENDIF ELSE IF Target > Mid_Pulse THEN Desired_Pos(3) = Min_Pulse ELSE Desired_Pos(3) = Max_Pulse ENDIF ENDIF GOTO 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 PULSOUT Index, ( Current_Pos(Index) ) NEXT RETURN '------------------------------------------ '-- 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