' {$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