Comments and changes to DVL files

sensors_and_controls/src/teledyne_dvl/readme.txt → sensors_and_controls/src/teledyne_dvl/OLDreadme.txt

@@ -1,21 +1,21 @@
-Testing the DVL
-CR1 
-ck
-PS0
-
-
-Deployment test
-pa
-
-
-so far
-pa test passed
-connection - passes, finnicky
-
-
-
-dvl_test.py
-	has coded the pt5 and pa tests
-	no changes to any data
-	unsure what the problems are
-
+Testing the DVL
+CR1 
+ck
+PS0
+
+
+Deployment test
+pa
+
+
+so far
+pa test passed
+connection - passes, finnicky
+
+
+
+dvl_test.py
+	has coded the pt5 and pa tests
+	no changes to any data
+	unsure what the problems are
+

sensors_and_controls/src/teledyne_dvl/README.md

@@ -0,0 +1,26 @@
+# Teledyne DVL
+This folder contains files and code related to the Teledyne Pathfinder DVL.
+
+The DVL will be connected to the computer and will directly communicate with the computer equivalent.
+
+`dvl_dummy.ino` is currently used to create fake dummy data to be send using an arduino. `dvl_dummy.ino` is used to create and send dummy data using `serial`. If using `dvl_dummy.ino` to create fake dummy data then change the argument in `main.py` to `/dev/ttyACM0` otherwise keep as `COM4` to communicate with the DVL.
+
+`dvl_driver.py` has yet to be tested with the Pathfinder DVL. Currently used to receive dummy data and print it out.
+
+The `main.py` file is to be used to run start things up.
+Change the port in `main.py` if necessary if you cannot connect to the current port.
+
+In the `main.py` file import either dvl_driver.py as DVL or dvl_test.py as DVL for testing purposes.
+
+## Getting Started
+### Requirements:
+- Python 2.7
+- Ros Melodic
+- Ubuntu 18.04
+- Possibly [rosserial](http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup)
+
+### To run the code
+- Go to root folder of project
+- `catkin_make`
+- `source devel/setup.bash`
+- `rosrun sensing_and_actuation main.py`

sensors_and_controls/src/teledyne_dvl/dvl_driver.py

@@ -18,24 +18,38 @@ def __init__(self):
         self.timestamp = 0
 
     def __str__(self):
-        return f'xvel={self.xvel}\tyvel={self.yvel}\tzvel={self.zvel}\n' \
-               f'xpos={self.xpos}\typos={self.ypos}\tzpos={self.zpos}\n' \
-               f'pitch={self.pitch}\tyaw{self.yaw}\troll={self.roll}\n'
+	# Temporary return statement
+	# Syntax error with other return statement
+	return 'xvel={0}\nyvel={1}\nzvel={2}\n' \
+		'xpos={3}\nypos={4}\nzpos={5}\n' \
+		'yaw={6}\npitch={7}\nroll={8}'.format(self.xvel, self.yvel, self.zvel, self.xpos, self.ypos, self.zpos, self.yaw, self.pitch, self.roll)
+
+	# Below return statement has f-string which was introduced in python 3.6
+        # return f'xvel={self.xvel}\tyvel={self.yvel}\tzvel={self.zvel}\n' \
+        #       f'xpos={self.xpos}\typos={self.ypos}\tzpos={self.zpos}\n' \
+        #       f'pitch={self.pitch}\tyaw{self.yaw}\troll={self.roll}\n'
 
 class DVL(Thread):
 
 
     def __init__(self, port):
         Thread.__init__(self)
-        self.serial = serial.Serial(port=port, baudrate=115200,
-                                    bytesize=8, timeout=2, stopbits=serial.STOPBITS_ONE)
+        self.serial = serial.Serial(port=port, baudrate=115200)
+	print('Port: {0}'.format(port))
         #for threading data purposes
         self.mutex = Lock()
 
         self.msg = Msg()
         self.data_flag = False
 
-    def write(self, output: str) -> None:
+    # Original write function
+    # Might be causing problems because the syntax (variable annotation) was introduced in a later version of python. Currently using 2.7
+    # def write(self, output: str) -> None:
+    #     self.serial.write(output.encode())
+
+    # New write function because syntax errors from the original function was causing problems
+    # Might not produce the same functionality as the previous version
+    def write(self, output):
         self.serial.write(output.encode())
 
     def has_data(self):
@@ -59,14 +73,15 @@ def run(self):
         # rospy.sleep(5)  # sleep for 2 seconds
 
         # ROS publisher setup
-        # pub = rospy.Publisher('dvl_status', DVL, queue_size=1)
-        # # pubHeading = rospy.Publisher('dvl_heading', Float32, queue_size = 1)
-        # pubSS = rospy.Publisher('dvl_ss', Float32, queue_size=1)
+        pub = rospy.Publisher('dvl_status', DVL, queue_size=1)
+        pubHeading = rospy.Publisher('dvl_heading', Float32, queue_size = 1)
+        pubSS = rospy.Publisher('dvl_ss', Float32, queue_size=1)
         # rospy.Subscriber('current_rotation', Rotation, self.rCallBack, queue_size=1)
         # msg = DVL()
         # # msgHeading = Float32()
-        # msgSS = Float32()
-
+        msgSS = Float32()
+
+	# Below required to setup settings and start communicating with DVL
         # PD6 settings --------------------------------------------------------------
         self.write("CR1\r")  # set factory defaults.(Pathfinder guide p.67)
         self.write("CP1\r")  # required command
@@ -100,14 +115,30 @@ def run(self):
 
         while True:
             loop_time = time.time()
-
             if self.serial.in_waiting > 0:  # If there is a message from the DVL
                 try:
                     line = self.serial.readline()
                     # print(line)
+
                     line = str(line)
+		    lineSplit = line.split(",")
+
                     # print(line)
-                    # print(line)
+		    # Code underneath sends to Msg class and prints
+		    self.msg.xvel = lineSplit[0]
+		    self.msg.yvel = lineSplit[1]
+		    self.msg.zvel = lineSplit[2]
+
+		    self.msg.xpos = lineSplit[3]
+		    self.msg.ypos = lineSplit[4]
+		    self.msg.zpos = lineSplit[5]
+
+		    self.msg.yaw = lineSplit[6]
+		    self.msg.pitch = lineSplit[7]
+		    self.msg.roll = lineSplit[8]
+
+		    print(self.msg.__str__())
+
                 except:
                     print("read fail")
                 #
@@ -133,7 +164,7 @@ def run(self):
                     self.depth = rangeToBottom
                     self.mutex.release()
 
-                    # pub.publish(msg)
+                    pub.publish(msg)
                     # print(self.__str__())
 
                 elif ":BS" in line:  # If the message is a velocity update
@@ -150,7 +181,7 @@ def run(self):
                     self.mutex.release()
 
                     # print(str(self))
-                    # pub.publish(msg)
+                    pub.publish(msg)
 
                 elif ":SA" in line:
                     line = line.split(",")
@@ -170,4 +201,4 @@ def run(self):
                     self.data_flag = True
                     self.mutex.release()
                     # print(f'timestamp: {float(line[5])}')
-                    # pubSS.publish(msgSS)
+                    pubSS.publish(msgSS)

sensors_and_controls/src/teledyne_dvl/dvl_dummy/dvl_dummy.ino

@@ -0,0 +1,48 @@
+/*
+ * rosserial Publishing DVL sensor data
+ */
+
+#include <ros.h>
+// INCLUDE NECESSARY LIBRARY FOR SENSORS
+
+int xvel = 0;
+int yvel = 0;
+int zvel = 0;
+int xpos = 0;
+int ypos = 0;
+int zpos = 0;
+int yaw = 0;
+int pitch = 0;
+int roll = 0;
+int depth = 0;
+String s = ",";
+
+void setup() {
+  //SHOULD INITIALIZE SENSORS BELOW IN THE SETUP
+  //Set serial baudrate to the same in dvl_driver.py
+  Serial.begin(115200);
+}
+
+void loop() {
+  //USE LOOP TO CONSTANTLY GET SENSOR DATA AND PUBLISH IT
+  //SHOULD HAVE A DELAY AS TO HOW OFTEN IT SHOULD GET DATA
+
+
+
+  //Serial printing to computer. Currently printing DUMMY DATA
+  //Printing dummy data in the form of xpos, ypos, zpos, yaw, pitch, roll, depth
+  Serial.println(xvel + s + yvel + s + zvel + s + xpos + s + ypos + s + zpos + s + yaw + s + pitch + s + roll);
+
+  //Randomly adding 
+  xvel = xvel + random(-1, 2);
+  yvel = yvel + random(-1, 2);
+  zvel = zvel + random(-1, 2);
+  xpos = xpos + random(-5, 6);
+  ypos = ypos + random(-5, 6);
+  zpos = zpos + random(-5, 6);
+  yaw = yaw + random(-3, 4);
+  pitch = pitch + random(-3, 4);
+  roll = roll + random(-3, 4);
+
+  delay(3000);
+}

sensors_and_controls/src/teledyne_dvl/main.py

@@ -1,25 +1,29 @@
-from dvl_driver import DVL
-import matplotlib.pyplot as plt
-from matplotlib import style
-
-
-def main():
-    # style.use('fivethirtyeight')
-    #
-    # fig = plt.figure()
-    # ax1 = fig.add_subplot(1,1,1)
-
-
-    dvl = DVL("Com4")
-    dvl.start()
-
-
-
-    while True:
-        if dvl.has_data():
-            data = dvl.get_data()
-            print(str(data))
-
-
-if __name__ == '__main__':
-        main()
+#! /usr/bin/env python
+import rospy
+
+
+from dvl_driver import DVL
+import matplotlib.pyplot as plt
+from matplotlib import style
+
+
+def main():
+    style.use('fivethirtyeight')
+
+    fig = plt.figure()
+    ax1 = fig.add_subplot(1,1,1)
+
+    # COM4 being the port which connects from the computer to DVL.
+    dvl = DVL("COM4")
+    dvl.start()
+
+
+
+    while True:
+        if dvl.has_data():
+            data = dvl.get_data()
+            print(str(data))
+
+
+if __name__ == '__main__':
+        main()