From 05c5c89944145bd4775106880d8acee6db3eb5d0 Mon Sep 17 00:00:00 2001
From: Eoin <crottye@wit.edu>
Date: Tue, 28 Jan 2020 16:29:44 -0500
Subject: [PATCH 1/2] Added config file, modified PCA and vector converter to
 use config file

---
 config/config.yaml                            | 30 +++++++++++
 .../vector_trajectory_converter.py            | 29 +++--------
 launch/config.launch                          | 13 +++++
 launch/core.launch                            |  2 +-
 launch/ieee2020.launch                        |  1 +
 plugins/motors/thruster/pca9685.py            | 52 +++++++++++++------
 6 files changed, 87 insertions(+), 40 deletions(-)
 create mode 100644 config/config.yaml
 create mode 100644 launch/config.launch

diff --git a/config/config.yaml b/config/config.yaml
new file mode 100644
index 0000000..e3989b1
--- /dev/null
+++ b/config/config.yaml
@@ -0,0 +1,30 @@
+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,
+            const_array_x: [[0.0,0.0,0.0,0.0],[1.0,-1.0,-1.0,1.0]],
+            const_array_y: [[0.0,0.0,0.0,0.0],[1.0,1.0,-1.0,-1.0]],
+            const_array_z: [[1.0,1.0,1.0,1.0],[0.0,0.0,0.0,0.0]],
+            const_array_roll: [[0.0,1.0,0.0,1.0],[0.0,0.0,0.0,0.0]],
+            const_array_pitch: [[1.0,0.0,-1.0,0.0],[0.0,0.0,0.0,0.0]],
+            const_array_cut: [[0.0,0.0,0.0,0.0],[1.0,-1.0,1.0,-1.0]],
+            arr_corrective: [[-1,1,-1,1],[1,1,-1,-1]]
+           }
diff --git a/core/trajectory_converter/vector_trajectory_converter.py b/core/trajectory_converter/vector_trajectory_converter.py
index 49a7c08..76a0e99 100755
--- a/core/trajectory_converter/vector_trajectory_converter.py
+++ b/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 = rospy.get_param('Thrusters/const_array_x') # x
 
-const_array_y = [
-    [0.0, 0.0, 0.0, 0.0], [1.0, 1.0, -1.0, -1.0]  # y
-]
+const_array_y = rospy.get_param('Thrusters/const_array_y') #y
 
-const_array_z = [
-    [1.0, 1.0, 1.0, 1.0], [0.0, 0.0, 0.0, 0.0]  # z
-]
+const_array_z = rospy.get_param('Thrusters/const_array_z') #z
 
-const_array_roll = [
-    [0.0, 1.0, 0.0, 1.0], [0.0, 0.0, 0.0, 0.0]  # roll
-]
+const_array_roll = rospy.get_param('Thrusters/const_array_roll') #roll
 
-const_array_pitch = [
-    [1.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 0.0]  # pitch
-]
+const_array_pitch = rospy.get_param('Thrusters/const_array_pitch') #pitch
 
-const_array_cut = [
-    [0.0, 0.0, 0.0, 0.0], [1.0, -1.0, 1.0, -1.0]  # cut
-]
+const_array_cut = rospy.get_param('Thrusters/const_array_cut') #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(
diff --git a/launch/config.launch b/launch/config.launch
new file mode 100644
index 0000000..4562f0b
--- /dev/null
+++ b/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>
diff --git a/launch/core.launch b/launch/core.launch
index ac7576f..47ca796 100644
--- a/launch/core.launch
+++ b/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>
\ No newline at end of file
+</launch>
diff --git a/launch/ieee2020.launch b/launch/ieee2020.launch
index 0854ed7..11dbbcc 100644
--- a/launch/ieee2020.launch
+++ b/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="" />
diff --git a/plugins/motors/thruster/pca9685.py b/plugins/motors/thruster/pca9685.py
index 018fd00..83d284b 100755
--- a/plugins/motors/thruster/pca9685.py
+++ b/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:
@@ -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:

From 19d55fdb9eafc45a497b09121701ef4c963f6f44 Mon Sep 17 00:00:00 2001
From: Eoin <crottye@wit.edu>
Date: Tue, 4 Feb 2020 22:16:03 -0500
Subject: [PATCH 2/2] Updated config file to remove array math, PCA now pulls
 max and min vals from config.yaml

---
 config/config.yaml                                   |  6 ------
 .../vector_trajectory_converter.py                   | 12 ++++++------
 plugins/motors/thruster/pca9685.py                   |  4 ++--
 3 files changed, 8 insertions(+), 14 deletions(-)

diff --git a/config/config.yaml b/config/config.yaml
index e3989b1..41e7f14 100644
--- a/config/config.yaml
+++ b/config/config.yaml
@@ -20,11 +20,5 @@ Thrusters: {top_front: -1,
             front_right: -1,
             back_left: -1,
             back_right: -1,
-            const_array_x: [[0.0,0.0,0.0,0.0],[1.0,-1.0,-1.0,1.0]],
-            const_array_y: [[0.0,0.0,0.0,0.0],[1.0,1.0,-1.0,-1.0]],
-            const_array_z: [[1.0,1.0,1.0,1.0],[0.0,0.0,0.0,0.0]],
-            const_array_roll: [[0.0,1.0,0.0,1.0],[0.0,0.0,0.0,0.0]],
-            const_array_pitch: [[1.0,0.0,-1.0,0.0],[0.0,0.0,0.0,0.0]],
-            const_array_cut: [[0.0,0.0,0.0,0.0],[1.0,-1.0,1.0,-1.0]],
             arr_corrective: [[-1,1,-1,1],[1,1,-1,-1]]
            }
diff --git a/core/trajectory_converter/vector_trajectory_converter.py b/core/trajectory_converter/vector_trajectory_converter.py
index 76a0e99..51ed2a7 100755
--- a/core/trajectory_converter/vector_trajectory_converter.py
+++ b/core/trajectory_converter/vector_trajectory_converter.py
@@ -29,17 +29,17 @@
 
 # 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 = rospy.get_param('Thrusters/const_array_x') # x
+const_array_x = [[0.0,0.0,0.0,0.0],[1.0,-1.0,-1.0,1.0]] # x
 
-const_array_y = rospy.get_param('Thrusters/const_array_y') #y
+const_array_y = [[0.0,0.0,0.0,0.0],[1.0,1.0,-1.0,-1.0]] #y
 
-const_array_z = rospy.get_param('Thrusters/const_array_z') #z
+const_array_z = [[1.0,1.0,1.0,1.0],[0.0,0.0,0.0,0.0]] #z
 
-const_array_roll = rospy.get_param('Thrusters/const_array_roll') #roll
+const_array_roll = [[0.0,1.0,0.0,1.0],[0.0,0.0,0.0,0.0]] #roll
 
-const_array_pitch = rospy.get_param('Thrusters/const_array_pitch') #pitch
+const_array_pitch = [[1.0,0.0,-1.0,0.0],[0.0,0.0,0.0,0.0]] #pitch
 
-const_array_cut = rospy.get_param('Thrusters/const_array_cut') #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
diff --git a/plugins/motors/thruster/pca9685.py b/plugins/motors/thruster/pca9685.py
index 83d284b..c2c17e6 100755
--- a/plugins/motors/thruster/pca9685.py
+++ b/plugins/motors/thruster/pca9685.py
@@ -357,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 '