Config

config/config.yaml

@@ -0,0 +1,24 @@
+PCA_Config: {MIN_PCA: 1850,
+             MAX_PCA: 3050,
+             PCA_FREQ: 390,
+             MAX_ATTEMPT_COUNT : 5,
+             #channels
+             top_front: 5,
+             top_back: 6,
+             top_left: -1,
+             top_right: -1,
+             front_left: 4,
+             front_right: 1,
+             back_left: 3,
+             back_right: 2
+            }
+Thrusters: {top_front: -1,
+            top_back: -1, 
+            top_left: -1,
+            top_right: -1,
+            front_left: -1,
+            front_right: -1,
+            back_left: -1,
+            back_right: -1,
+            arr_corrective: [[-1,1,-1,1],[1,1,-1,-1]]
+           }

core/trajectory_converter/vector_trajectory_converter.py

@@ -29,36 +29,21 @@
 
 # These arrays are in the format:
 # [top front, top right, top back, top left], [front left, front right, back right, back left]
-const_array_x = [
-    [0.0, 0.0, 0.0, 0.0], [1.0, -1.0, -1.0, 1.0]  # x
-]
+const_array_x = [[0.0,0.0,0.0,0.0],[1.0,-1.0,-1.0,1.0]] # x
 
-const_array_y = [
-    [0.0, 0.0, 0.0, 0.0], [1.0, 1.0, -1.0, -1.0]  # y
-]
+const_array_y = [[0.0,0.0,0.0,0.0],[1.0,1.0,-1.0,-1.0]] #y
 
-const_array_z = [
-    [1.0, 1.0, 1.0, 1.0], [0.0, 0.0, 0.0, 0.0]  # z
-]
+const_array_z = [[1.0,1.0,1.0,1.0],[0.0,0.0,0.0,0.0]] #z
 
-const_array_roll = [
-    [0.0, 1.0, 0.0, 1.0], [0.0, 0.0, 0.0, 0.0]  # roll
-]
+const_array_roll = [[0.0,1.0,0.0,1.0],[0.0,0.0,0.0,0.0]] #roll
 
-const_array_pitch = [
-    [1.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 0.0]  # pitch
-]
+const_array_pitch = [[1.0,0.0,-1.0,0.0],[0.0,0.0,0.0,0.0]] #pitch
 
-const_array_cut = [
-    [0.0, 0.0, 0.0, 0.0], [1.0, -1.0, 1.0, -1.0]  # cut
-]
+const_array_cut = [[0.0,0.0,0.0,0.0],[1.0,-1.0,1.0,-1.0]] #cut
 
 # In the event that a thruster is backwards, or running too fast, it can be corrected here.
-# TODO: get these from a config file
-arr_corrective = [
-    [-1, -1, -1, -1], [-1, 1, 1, -1]
-]
 
+arr_corrective = rospy.get_param('Thrusters/arr_corrective')
 
 def print_array(array):
     rospy.loginfo(

launch/config.launch

@@ -0,0 +1,13 @@
+<launch>
+    <rosparam command="load" file="$(find auv)/config/config.yaml" />
+    <node name="pca9685" pkg="auv" type="pca9685.py" />
+    <node name="ninedof" pkg="auv" type="fxas_fxos.py" />
+    <node name="thruster_tester" pkg="auv" type="individual_thruster_control_pca.py" args="" />
+
+
+    <node name="servo" pkg="auv" type="servo_by_pca.py" args="servo 11" />
+    <node name="stepper" pkg="auv" type="stepper_by_pca.py" args="stepper 12" />
+
+    <!-- It's usually easier to tack this on last (to keep error message line numbers accurate) -->
+    <include file="$(find auv)/launch/core.launch" />
+</launch>

launch/core.launch

@@ -3,4 +3,4 @@
     <node name="thruster_converter" pkg="auv" type="vector_trajectory_converter.py" args="" />
     <node name="filtering" pkg="auv" type="rolling_average.py" />
     <node name="control_loop" pkg="auv" type="control_loop_pi.py" />
-</launch>
+</launch>

launch/ieee2020.launch

@@ -1,4 +1,5 @@
 <launch>
+    <rosparam command="load" file="$(find auv)/config/config.yaml" />
     <node name="pca9685" pkg="auv" type="pca9685.py" args="1850 3050 --frequency 390 --top_front 5 --top_back 6 --front_right 1 --back_right 2 --back_left 3 --front_left 4" />
     <node name="ninedof" pkg="auv" type="fxas_fxos.py" />
     <node name="thruster_tester" pkg="auv" type="individual_thruster_control_pca.py" args="" />

plugins/motors/thruster/pca9685.py

@@ -25,25 +25,27 @@
 from auv.msg import thruster_sensor, thrustermove, arbitrary_pca_commands
 from threading import Thread
 
+
+
 # The thruster_dictionary takes sensible thruster names and turns them into PCA channels.
 # We default to -1 as a flag value.
 thruster_dictionary = {
-    'top_front': -1,
-    'top_back': -1,
-    'top_left': -1,
-    'top_right': -1,
-    'front_left': -1,
-    'front_right': -1,
-    'back_left': -1,
-    'back_right': -1
+    'top_front': rospy.get_param("Thrusters/top_front"),
+    'top_back': rospy.get_param("Thrusters/top_back"),
+    'top_left': rospy.get_param("Thrusters/top_left"),
+    'top_right': rospy.get_param("Thrusters/top_right"),
+    'front_left': rospy.get_param("Thrusters/front_left"),
+    'front_right': rospy.get_param("Thrusters/front_right"),
+    'back_left': rospy.get_param("Thrusters/back_left"),
+    'back_right': rospy.get_param("Thrusters/back_right")
 }
 
 dead_thrusters = []
 
-MAX_ATTEMPT_COUNT = 5
-MIN_PCA_INT_VAL = None
-MAX_PCA_INT_VAL = None
-PCA_FREQ_VAL = 400
+MAX_ATTEMPT_COUNT = rospy.get_param("PCA_Config/MAX_ATTEMPT_COUNT")
+MIN_PCA_INT_VAL = rospy.get_param("PCA_Config/MIN_PCA")
+MAX_PCA_INT_VAL = rospy.get_param("PCA_Config/MAX_PCA")
+PCA_FREQ_VAL = rospy.get_param("PCA_Config/PCA_FREQ")
 PCA_CONTROL_LOCK = False
 
 try:
@@ -355,9 +357,9 @@ def listener(arguments):
     # 'parser' allows us to receive and parse command line arguments.
     parser = argparse.ArgumentParser("Create a ROS interface for the PCA9685 hardware to control thrusters and other "
                                      "motors.")
-    parser.add_argument('min_pca_int_value', type=int,
+    parser.add_argument('--min_pca_int_value', type=int,
                         help='Integer value that represents the lowest value to be passed to the PCA.')
-    parser.add_argument('max_pca_int_value', type=int,
+    parser.add_argument('--max_pca_int_value', type=int,
                         help='Integer value that represents the highest value to be passed to the PCA.')
     parser.add_argument('--frequency', type=int,
                         help='The value to be passed to the set_pwm_freq() function. Note that this should produce a '
@@ -384,11 +386,27 @@ def listener(arguments):
     # ROS likes to slap some other command line args if we're running from roslaunch/launchfile. This allows us to
     # strip those away so we're just looking at what we've passed in.
     args = parser.parse_args(rospy.myargv()[1:])
-
-    MAX_PCA_INT_VAL = args.max_pca_int_value
-    MIN_PCA_INT_VAL = args.min_pca_int_value
-
-    # Set the values to map thrusters to PCA channels.
+    # set the pca min and max values using config.
+    MAX_PCA_INT_VAL = rospy.get_param('PCA_Config/MAX_PCA')
+    MIN_PCA_INT_VAL = rospy.get_param('PCA_Config/MIN_PCA')
+
+    # set the pca min and max values using flags
+    if args.max_pca_int_value is not None:
+        MAX_PCA_INT_VAL = args.max_pca_int_value
+    if args.min_pca_int_value is not None:
+        MIN_PCA_INT_VAL = args.min_pca_int_value
+    # set the values to map thruster to PCA channels using config.
+    thruster_dictionary['top_front'] = rospy.get_param('PCA_Config/top_front')
+    thruster_dictionary['top_right'] = rospy.get_param('PCA_Config/top_right')
+    thruster_dictionary['top_back'] = rospy.get_param('PCA_Config/top_back')
+    thruster_dictionary['top_left'] = rospy.get_param('PCA_Config/top_left')
+    thruster_dictionary['front_right'] = rospy.get_param('PCA_Config/front_right')
+    thruster_dictionary['front_left'] = rospy.get_param('PCA_Config/front_left')
+    thruster_dictionary['back_right'] = rospy.get_param('PCA_Config/back_right')
+    thruster_dictionary['back_left'] = rospy.get_param('PCA_Config/back_left')
+
+
+    # Set the values to map thrusters to PCA channels using flags.
     if args.top_front is not None:
         thruster_dictionary['top_front'] = args.top_front
     if args.top_right is not None: