diff --git a/scripts/auto.py b/scripts/auto.py index 0b74db5..2dd7a26 100755 --- a/scripts/auto.py +++ b/scripts/auto.py @@ -7,6 +7,7 @@ from nav_msgs.msg import Odometry from nova_common.msg import * from nova_common.srv import * +import steeringPID testing = True #--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**-- @@ -132,6 +133,10 @@ def waypointCallback(waypointData): lng = waypointData.lng waypoint.setCoords(lat,lng) +def pdiSteer(controller, rawCommand): + controller.ComputeTurn(rawCommand) + return controller.turn_est + #--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**-- # getMode(): Retrieve Mode from parameter server. #--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..-- @@ -149,7 +154,7 @@ def waypointCallback(waypointData): # Main function #--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..-- def auto(): - + pdiSteering = steeringPID(0,2) global rovey_pos global waypoint global auto_engaged @@ -186,10 +191,9 @@ def auto(): # steer_limit = rospy.get_param('steer_limit') drive_msg = DriveCmd() - drive_msg.rpm = 10 - drive_msg.steer_pct = turn * 0.5 - drive_pub.publish(drive_msg) - + drive_msg.steer_pct = turn + drive_pub.publish(pdiSteer(pdiSteering, turn)) + drive_msg.rpm = 0 #if distance < : # desPos.set_coords(route[1][0], route[1][1]) diff --git a/scripts/auto_controller.py b/scripts/auto_controller.py index 756e0d4..6b5abeb 100755 --- a/scripts/auto_controller.py +++ b/scripts/auto_controller.py @@ -105,11 +105,11 @@ def auto_drive(self): rospy.loginfo("distance: %s", distance) rospy.loginfo("orientation: %s", self.orientation) rospy.loginfo("current pos: %s", self.waypoint) - rpm_limit = rospy.get_param('rpm_limit',100) - steer_limit = rospy.get_param('steer_limit',100) + rpm_limit = rospy.get_param('rpm_limit',10) + steer_limit = rospy.get_param('steer_limit',10) drive_msg = DriveCmd() - drive_msg.rpm = rpm_limit*10 - drive_msg.steer_pct = steer_limit*10*turn/180 + drive_msg.rpm = 10 + drive_msg.steer_pct = 0.5*turn self.drive_pub.publish(drive_msg) def metricCalculation(self): self.orientation = self.rovey_pos.yaw @@ -208,7 +208,7 @@ def auto_controller(): ## testing if testing: time.sleep(2) - SM.des_pos = WaypointClass(-37.660970, 145.368935) + SM.des_pos = WaypointClass(-37.6617819, 145.3692175) SM.toTraverse() ## end testing while not rospy.is_shutdown(): diff --git a/scripts/auto_functions.py b/scripts/auto_functions.py index e2e8c9d..b964b7b 100644 --- a/scripts/auto_functions.py +++ b/scripts/auto_functions.py @@ -1,5 +1,5 @@ import rospy, math,numpy -from auto_classes import WaypointClass +from auto_classes import WaypointClass #--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**-- # bearingInDegrees(): # Calculates the direction an object is pointing in relation to the @@ -104,6 +104,6 @@ def spiralSearch(current_pos,no_of_waypoints,rang_min,rang_max): lng=r*numpy.cos(theta) + current_pos.longitude lat=r*numpy.sin(theta) + current_pos.latitude searchPath=list() - for i in range(len(x)): - searchPath.append(RoveyPosClass(lat[i],lng[i])) + for i in range(len(theta)): + searchPath.append(WaypointClass(lat[i],lng[i])) return searchPath diff --git a/scripts/ert.py b/scripts/ert.py new file mode 100755 index 0000000..34376f3 --- /dev/null +++ b/scripts/ert.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python +import rospy +import math +import time +from sensor_msgs.msg import NavSatFix, MagneticField, Imu +from webots_ros.srv import set_float +from nova_common.msg import * +from nova_common.srv import * + +class RoveyPosClass(object): + + def __init__(self, lat, lng, roll, pitch, yaw): + self.latitude = lat + self.longitude = lng + self.roll = roll + self.pitch = pitch + self.yaw = yaw + + def setCoords(self, lat, lng): + self.latitude = lat + self.longitude = lng + + def setOrientation(self, roll, pitch, yaw): + self.roll = roll + self.pitch = pitch + self.yaw = yaw + +def distanceBetween(lat1, lng1, lat2, lng2): + distance = math.sqrt((lat2-lat1)**2 + (lng2-lng1)**2) + return distance + +def turnDirection(beta, orientation): + if beta < 180: + if (orientation < (beta+180)) & (orientation > beta): + rospy.logdebug("turn left") + turn = (orientation-beta) * -1 + elif orientation < beta: + rospy.logdebug("turn right") + turn = beta-orientation + else: + rospy.logdebug("turn right") + turn = (360-orientation)+beta + else: + if (orientation > (beta-180)) & (orientation < beta): + rospy.logdebug("turn right") + turn = beta-orientation + elif orientation > beta: + rospy.logdebug("turn left") + turn = (orientation-beta) * -1 + else: + rospy.logdebug("turn left") + turn = ((360-beta)+orientation) * -1 + + rospy.loginfo("turn: %s", turn) + return turn + +def angleBetween(lat1, lng1, lat2, lng2): + lat1 = math.radians(lat1) + lat2 = math.radians(lat2) + + longDiff = math.radians(lng2 - lng1) + y = math.sin(longDiff) * math.cos(lat2) + x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(longDiff) + + beta = math.atan2(y, x) + beta = math.degrees(beta) + beta = (beta+360) % 360 + + return beta + +def gpsCallback(gpsData): + global rovey_pos + lat = gpsData.latitude + lng = gpsData.longitude + rovey_pos.setCoords(lat,lng) + #rospy.logdebug("lat: %s, long: %s", lat, lng) + +def rpyCallback(rpyData): + global rovey_pos + rovey_pos.setOrientation(rpyData.roll, rpyData.pitch, rpyData.yaw) + +rovey_pos = RoveyPosClass(0,0,0,0,0) + +def ert(): + + global rovey_pos + + rospy.init_node('auto', anonymous=True) + rate = rospy.Rate(2) # Loop rate in Hz + + #how to get north direction from world info? or gps ref point? + + gps_sub = rospy.Subscriber("/nova_common/gps_data", NavSatFix, gpsCallback) + rpy_sub = rospy.Subscriber("/nova_common/RPY", RPY, rpyCallback) + status_pub = rospy.Publisher("/core_rover/auto_status", AutoStatus, queue_size=10) + + waypointlat = float(input("Input lat pls:")) + waypointlon = float(input("Input lon pls:")) + time.sleep(5) + while not rospy.is_shutdown(): + + orientation = rovey_pos.yaw + + if (True): + + beta =angleBetween(rovey_pos.latitude, rovey_pos.longitude, waypointlat, waypointlon) + distance = 110000 * distanceBetween(rovey_pos.latitude, rovey_pos.longitude, waypointlat, waypointlon) + turn = turnDirection(beta, orientation) + + rospy.loginfo("fish") + rospy.loginfo("beta: %s", beta) + rospy.loginfo("distance: %s", distance) + rospy.loginfo("orientation: %s", orientation) + + status_msg = AutoStatus() + status_msg.latitude = rovey_pos.latitude + status_msg.longitude = rovey_pos.longitude + status_msg.bearing = orientation + status_pub.publish(status_msg) + + rate.sleep() + +if __name__ == '__main__': + auto() diff --git a/scripts/field_test_19-04_imu_gps.bag~refs/remotes/origin/dev/autonomous b/scripts/field_test_19-04_imu_gps.bag~refs/remotes/origin/dev/autonomous new file mode 100644 index 0000000..5ddf021 Binary files /dev/null and b/scripts/field_test_19-04_imu_gps.bag~refs/remotes/origin/dev/autonomous differ diff --git a/scripts/gst_camera.py b/scripts/gst_camera.py index 0bdd7f0..6b0cc0b 100644 --- a/scripts/gst_camera.py +++ b/scripts/gst_camera.py @@ -18,22 +18,40 @@ Gst.init(sys.argv) - - # Number of feed types available -numFeedTypes = 6 +numFeedTypes = 7 + # Create an enum of different feed types # Enum Name = Index class FeedType (Enum): - FT_Stereo = 0 - FT_Telescopic = 1 - FT_FoscamBlack = 2 - FT_FoscamWhite = 3 - FT_Arm_1 = 4 - FT_Arm_2 = 5 - + FT_Stereo_Dual = 0 + FT_Stereo_Single = 1 + FT_Telescopic = 2 + FT_FoscamBlack = 3 + FT_FoscamWhite = 4 + FT_Arm_1 = 5 + FT_Arm_2 = 6 + + +# Create an enum for different quality types +# Enum Name = Input +class QualityType (Enum): + QT_High = 0 + QT_Medium = 1 + QT_Low = 2 + + +# Returns the factor for multiplying the quality for width and height +def QualityFactor (qf): + if qf == QualityType.QT_Medium: + return 0.5 + elif qf == QualityType.QT_Low: + return 0.25 + else: + return 1 + @@ -61,8 +79,10 @@ def on_message(bus, message, loop): # Stores a collection of all GST pipelines and buses # A bus stores the messages from GST and connects the terminal to command +# Camera states store the whether each camera is streaming or not pipelines = [None] * numFeedTypes buses = [None] * numFeedTypes +cameraStates = [False] * numFeedTypes @@ -73,13 +93,14 @@ def on_message(bus, message, loop): height = 500 frame_rate = 30 img_format = "I420" +cur_qualityType = QualityType.QT_High # Networking Variables ip_end = 8 port = '5000' # Camera Identification Variables -cur_feedType = FeedType.FT_Stereo +cur_feedType = FeedType.FT_Stereo_Dual video_IDs = [1,2] device_name = "Stereo Vision 2" isUSB = True @@ -90,22 +111,24 @@ def on_message(bus, message, loop): # Returns the GST pipeline with updated variable values for Stereo Cam def gst_pipeline_stereo(): - return "v4l2src device=/dev/video{} ! videoscale ! video/x-raw, width={}, height={}, framerate={}/1, format={} ! compositor name=comp sink_1::xpos={} ! jpegenc ! rtpjpegpay ! udpsink host=192.168.1.{} port={} v4l2src device=/dev/video{} ! videoscale ! video/x-raw, width={}, height={}, framerate={}/1, format={} ! comp.".format(video_IDs[0], width, height, frame_rate, img_format, width, ip_end, port, video_IDs[1], width, height, frame_rate, img_format) + qual = QualityFactor(cur_qualityType) + return "v4l2src device=/dev/video{} ! videoscale ! video/x-raw, width={}, height={}, framerate={}/1, format={} ! compositor name=comp sink_1::xpos={} ! jpegenc ! rtpjpegpay ! udpsink host=192.168.1.{} port={} v4l2src device=/dev/video{} ! videoscale ! video/x-raw, width={}, height={}, framerate={}/1, format={} ! comp.".format(video_IDs[0], int(width * qual), int(height * qual), frame_rate, img_format, width, ip_end, port, video_IDs[1], width * qual, height * qual, frame_rate, img_format) -# Returns the GST pipeline with updated variable values for Arm Cam -def gst_pipeline_arm(): - return "v4l2src device=/dev/video{} ! videoscale ! video/x-raw, width={}, height={}, framerate={}/1, format={} ! jpegenc ! rtpjpegpay ! udpsink host=192.168.1.{} port={}".format(video_IDs[0], width, height, frame_rate, img_format, ip_end, port) +# Returns the GST pipeline with updated variable values for single USB Cameras +def gst_pipeline_single(): + qual = QualityFactor(cur_qualityType) + return "v4l2src device=/dev/video{} ! videoscale ! video/x-raw, width={}, height={}, framerate={}/1, format={} ! jpegenc ! rtpjpegpay ! udpsink host=192.168.1.{} port={}".format(video_IDs[0], int(width * qual), int(height * qual), frame_rate, img_format, ip_end, port) # Returns the GST pipeline with updated variable values for Foscam (with IP) def gst_pipeline_foscam(_foscamID): - #return("gst-launch-1.0 rtspsrc location=rtsp://nova:rovanova@192.168.1.{}:88/videoMain ! autovideosink".format(_foscamID)) - return("rtspsrc location=rtsp://nova:rovanova@192.168.1.{}:88/videoMain ! decodebin ! videoscale ! video/x-raw, width=640, height=480 ! jpegenc ! rtpjpegpay ! udpsink host=192.168.1.{} port={} sync=false".format(_foscamID, ip_end, port)) + qual = QualityFactor(cur_qualityType) + return("rtspsrc location=rtsp://nova:rovanova@192.168.1.{}:88/videoMain ! decodebin ! videoscale ! video/x-raw, width={}, height={} ! jpegenc ! rtpjpegpay ! udpsink host=192.168.1.{} port={} sync=false".format(_foscamID, int(width * qual), int(height * qual), ip_end, port)) @@ -131,37 +154,129 @@ def gst_pipeline_foscam(_foscamID): isRunning = True -print("Running GStreamer camera stream window. Type 'help' or 'h' for more information.") - # While program is running, watch for commands while isRunning: + + # Add header information: + os.system('clear') # Clear window + print("Running GStreamer camera stream window. Type 'help' or 'h' for more information.") + + # Get current streaming cameras + camStreams = 'None' + for idx, val in enumerate(cameraStates): + if val == True: + camStreams += ', {}'.format(idx) + + # If there are cameras streaming + if camStreams != 'None': + camStreams = camStreams[6:] + + print("Current Streaming Camera Indexes: [{}]".format(camStreams)) + + print("Quality Settings: {}\nIPv4 Address: 192.168.1.{}".format(cur_qualityType, ip_end)) + + # Get user command command = str(raw_input('\n' + - '****************************\n' + + '*******************************************\n' + "Enter a new command ('h' to get help): ")).lower() + + + # Help command if command == 'h' or command == 'help': print( - '\n****************************' + + '\n*******************************************' + '\n Program Command List ' + - '\n****************************' + + '\n*******************************************' + "\nExit Program:\t'e', 'exit'" + "\nHelp:\t\t'h', 'help'" + + "\nInformation:\t'.', 'info'" + + "\nChange IP:\t'i', 'ip'" + + "\nChange Quality:\t'q', 'quality'" + "\nStart Feed:\t's', 'start'" + "\nStop Feed:\t'x', 'stop'" + - '\n') + '\n*******************************************') + # Exit command if command == 'e' or command == 'exit': isRunning = False - print('Closing Program.') + print('\nClosing Program. \n') + + + + + # Information command + if command == '.' or command == 'info': + print('\nThis script is for setting up camera feeds across a network. The default IP is 192.168.1.X, where X can be defined using the IP menu. For each camera that is streamed, the stream can also be closed and the quality of the feed can be adjusted. Please note, when the quality or IP ending value is adjusted, the streams that are running will not automatically update. To update, close and start the stream again once the settings have been adjusted.') + + + + + # IP command + if command == 'i' or command == 'ip': + try: + ip_input = int(raw_input( + '\n*******************************************\n' + + 'Current IP: 192.168.1.{}\n'.format(ip_end) + + 'Enter a new IP end value: ' + )) + + ip_end = ip_input + except: + print('Invalid number entry.') + + #print break + print('*******************************************\n') + + + + + # Quality command + if command == 'q' or command == 'quality': + + # Get user input for quality type: + qual_input = str(raw_input( + '\n*******************************************\n' + + 'Press (H) for High\n' + + 'Press (M) for Medium\n' + + 'Press (L) for Low\n' + + '\t: ')).lower() + + # Print break + print('****************************\n') + + # Change quality type depending on user input + if qual_input == 'h': + # High Quality + cur_qualityType = QualityType.QT_High + elif qual_input == 'm': + # Medium Quality + cur_qualityType = QualityType.QT_Medium + elif qual_input == 'l': + # Low Quality + cur_qualityType = QualityType.QT_Low + else: + # Invalid Entry + print('Invalid Quality Settings Entered.') + continue + + print('Quality Settings Adjusted to {}'.format(cur_qualityType)) + + + +################################## +######### CHANGING FEED ########## +################################## # Start / Stop Feed command if command in ('s',' start', 'x', 'stop'): # Get user input for camera feed type: feed_input = str(raw_input( - '****************************\n' + + '\n*******************************************\n' + + 'Press (D) for Dual Stereo Cam\n' + 'Press (S) for Stereo Cam\n' + 'Press (T) for Telescopic Cam\n' + 'Press (B) for Black Foscam\n' + @@ -171,7 +286,7 @@ def gst_pipeline_foscam(_foscamID): '\t: ')).lower() # Print break - print('****************************\n') + print('*******************************************') # Change the feed type depending on user input if feed_input == 't': @@ -182,7 +297,7 @@ def gst_pipeline_foscam(_foscamID): height = 480 port = '5001' isUSB = True - cam_index = 1 + cam_index = 2 elif feed_input == '1': # Change to Arm Camera 1 feed @@ -192,7 +307,7 @@ def gst_pipeline_foscam(_foscamID): height = 480 port = '5004' isUSB = True - cam_index = 4 + cam_index = 5 elif feed_input == '2': # Change to Arm Camera 2 feed @@ -202,7 +317,7 @@ def gst_pipeline_foscam(_foscamID): height = 480 port = '5005' isUSB = True - cam_index = 5 + cam_index = 6 elif feed_input == 'b': # Change to Black Foscam feed @@ -212,7 +327,7 @@ def gst_pipeline_foscam(_foscamID): height = 480 port = 5002 isUSB = False - cam_index = 2 + cam_index = 3 elif feed_input == 'w': # Change to White Foscam feed @@ -222,23 +337,38 @@ def gst_pipeline_foscam(_foscamID): height = 480 port = 5003 isUSB = False - cam_index = 3 + cam_index = 4 + + elif feed_input == 's': + # Change to Single Stereo feed + cur_feedType = FeedType.FT_Stereo_Single + device_name = "Stereo Vision 2" + width = 720 + height = 500 + port = 5000 + isUSB = True + cam_index = 1 - else: - # Change to Stereo feed - cur_feedType = FeedType.FT_Stereo + elif feed_input == 'd': + # Change to Dual Stereo feed + cur_feedType = FeedType.FT_Stereo_Dual device_name = "Stereo Vision 2" width = 720 height = 500 port = 5000 isUSB = True cam_index = 0 + + else: + # No value pressed + continue - # Start Feed command - if command in ('s',' start'): + # Start Feed command + if command in ('s',' start'): + # Search /dev/ for video devices connected to rover bash_cmd = "ls /dev/ | grep video" output = subprocess.check_output(['bash','-c', bash_cmd]) @@ -256,8 +386,6 @@ def gst_pipeline_foscam(_foscamID): output = subprocess.check_output(['bash','-c', bash_cmd]) ids.append(output.strip("\n")) - - # Creates the list of desired camera devices from the dev list devices = [] for idx, val in enumerate(ids): @@ -275,22 +403,24 @@ def gst_pipeline_foscam(_foscamID): # Check for error in camera detection if len(devices) > 0 or not isUSB: - # Get the appropriate GST pipeline command - if cur_feedType == FeedType.FT_Stereo: + # Get the appropriate GST pipeline command + if cur_feedType == FeedType.FT_Stereo_Dual: gstCode = gst_pipeline_stereo() + elif cur_feedType == FeedType.FT_Stereo_Single: + gstCode = gst_pipeline_single() elif cur_feedType == FeedType.FT_Telescopic: - gstCode = gst_pipeline_arm() + gstCode = gst_pipeline_single() elif cur_feedType == FeedType.FT_FoscamBlack: - gstCode = gst_pipeline_foscam(53) + gstCode = gst_pipeline_foscam(53) elif cur_feedType == FeedType.FT_FoscamWhite: - gstCode = gst_pipeline_foscam(52) + gstCode = gst_pipeline_foscam(52) elif cur_feedType == FeedType.FT_Arm_1: - gstCode = gst_pipeline_arm() + gstCode = gst_pipeline_single() elif cur_feedType == FeedType.FT_Arm_2: - gstCode = gst_pipeline_arm() + gstCode = gst_pipeline_single() - # Create pipeline + # Create pipeline pipeline = Gst.parse_launch(gstCode) bus = pipeline.get_bus() @@ -307,17 +437,47 @@ def gst_pipeline_foscam(_foscamID): # Start the pipeline in the playing state pipelines[cam_index].set_state(Gst.State.PLAYING) + + # Update the camera state variable + cameraStates[cam_index] = True # If camera not plugged in, output error message else: print("Camera source '{}' not found. Please check to see if it is correctly plugged in.".format(device_name)) + + + + + + # Stop Feed command if command in ('x', 'stop'): if pipelines[cam_index] != None: pipelines[cam_index].set_state(Gst.State.NULL) print('Stopping stream from {} at IP = 192.168.1.{}:{}\n'.format(device_name, ip_end, port)) + cameraStates[cam_index] = False else: print('No stream playing at this feed.') + + # Waits for enter to be pressed + if isRunning: + raw_input('\nPress Enter to continue...\n') + + + + + + + + + + + + + + + + diff --git a/scripts/navigation.py b/scripts/navigation.py index e48bc94..568fab2 100755 --- a/scripts/navigation.py +++ b/scripts/navigation.py @@ -156,8 +156,13 @@ def initNavigation(): global waypoint_iter global spiral_engaged spiral_engaged = False - #des_pos.setCoords(-37.91008843314037 ,145.1362295348945) - des_pos.setCoords(req.latitude, req.longitude) + if testing: + #des_pos.setCoords(-37.91008843314037 ,145.1362295348945) + des_pos.setCoords(-37.6617318, 145.3689761) + + #des_pos.setCoords(-37.9103870 , 145.1360253) + else: + des_pos.setCoords(req.latitude, req.longitude) auto_engaged = True waypoint_list = wayPoint(rovey_pos.longitude,rovey_pos.latitude,des_pos.longitude,des_pos.latitude,4) waypoint_iter = iter(waypoint_list) diff --git a/scripts/rover_sync.py b/scripts/rover_sync.py index b5e48ef..c5a5182 100755 --- a/scripts/rover_sync.py +++ b/scripts/rover_sync.py @@ -3,6 +3,7 @@ import rospy import rospkg import roslaunch +import os from rover_sm import RoverStateMachine from nova_common.msg import * from nova_common.srv import * @@ -14,10 +15,8 @@ class RoverSync: # __init__(): # Initialise class. #--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..-- - def __init__(self): - - rospy.init_node('rover_sync') - + def __init__(self, parent=None): + #super(RoverSync,self).__init__(parent) self.req_change_mode_server = rospy.Service( '/core_rover/req_change_mode', ChangeMode, self.handleReqChangeMode) @@ -56,17 +55,7 @@ def handleReqChangeMission(self, req): name = "aut" if (name is not "None"): - run_id = rospy.get_param("/run_id") - uuid = roslaunch.rlutil.get_or_generate_uuid(run_id, True) - roslaunch.configure_logging(uuid) - - rospack = rospkg.RosPack() # Get the file path for nova_common - path = rospack.get_path('nova_common') - - launch_file = [path + '/launch/{}.launch'.format(name)] - - self.launch = roslaunch.parent.ROSLaunchParent(uuid, launch_file) - self.launch.start() # Start the launch file + os.system("roslaunch nova_common {}.launch".format(name)) if success: rospy.set_param('/core_rover/Mission', req.mission) @@ -113,6 +102,7 @@ def handleReqChangeMode(self, req): # Main function. #--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..-- def main(): + rospy.init_node('rover_sync') rover_sync = RoverSync() rate = rospy.Rate(0.1) diff --git a/scripts/steeringPID.py b/scripts/steeringPID.py new file mode 100644 index 0000000..1cb8ff3 --- /dev/null +++ b/scripts/steeringPID.py @@ -0,0 +1,45 @@ +## A class that applies a PID controller to the steering of the nova rover +# in order to keep motion smooth and +import math +class steeringPID: + def __init__(self, first_value,frequency): + self.turn_est = first_value + self.freq = frequency + self.filteredTurn = lpf(1,frequency,first_value) + self.kp = 0.1 + self.kI = 0.03 + self.kd = 0.4 + self.kiMax = 1 + self.turn_est = 0 + self.freq = 1 + self.derTerm = 0 + self.propTerm = 0.0 + self.intTerm = 0.0 + def ComputeTurn(self, raw_command): + self.computeder(raw_command) + self.filteredTurn.stateUpdate(raw_command) + self.propTerm = self.filteredTurn.state + self.computeInt(raw_command) + self.turn_est = self.kd*self.derTerm + self.kp*self.propTerm + self.kI*self.intTerm + + def computeder(self,raw_value): + return self.freq* (raw_value - self.turn_est)*-1 + + def computeInt(self,raw_value): + self.intTerm = self.intTerm + raw_value + if self.intTerm*self.intTerm > self.kiMax: + self.intTerm = 1 +## Usage of a lpf here to reduce the likelihood of large spikes in the yaw estimate +# Causing hard turning of the motors, and possibly damaging them. +class lpf: + def __init__(self, timeConstant, freq, initialValue): + self.timeConstant = timeConstant + self.freq = freq + self.state = initialValue + self.a = math.exp(-self.timeConstant*self.freq) + + + def stateUpdate(self,newValue): + self.state = self.a*newValue + (1-self.a)*self.state + + \ No newline at end of file diff --git a/scripts/test.py b/scripts/test.py new file mode 100644 index 0000000..62412b3 --- /dev/null +++ b/scripts/test.py @@ -0,0 +1,16 @@ +## script to test the pid controller. +from steeringPID import steeringPID +import time +def pdiSteer(controller, rawCommand): + controller.ComputeTurn(rawCommand) + print controller.turn_est + return controller.turn_est +target = 1 +state = 0 +pdiSteering = steeringPID(0,2) +while(1): + effort = pdiSteer(pdiSteering,target-state) + state = state + effort + print("effort was", effort) + print("State is:",state) + time.sleep(0.1) \ No newline at end of file diff --git a/scripts/test/field_test_19-04_imu_gps.bag~refs/remotes/origin/dev/autonomous b/scripts/test/field_test_19-04_imu_gps.bag~refs/remotes/origin/dev/autonomous new file mode 100644 index 0000000..5ddf021 Binary files /dev/null and b/scripts/test/field_test_19-04_imu_gps.bag~refs/remotes/origin/dev/autonomous differ diff --git a/src/driver.cpp b/src/driver.cpp index 7cda500..28c90a4 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -61,7 +61,9 @@ int steer; int num; bool hbeat = false; int hbeat_cnt = 0; - +double prev_left = 0.0; +double prev_right = 0.0; +double max_delta = 0.04; ros::NodeHandle *n; // Create node handle to talk to ROS //Declaring and creating talonSRX objects to control the 6 motors. @@ -88,8 +90,7 @@ void DriveCmdCb(const nova_common::DriveCmd::ConstPtr& msg) { //Instantiiating vars double speedFactor = 2.0; //currently not in use, but can be used to increase/decrease speed by a factor - - speed = msg->rpm * 2; //speed = forward or reverse + speed = msg->rpm * 1; //speed = forward or reverse steer = msg->steer_pct; //steer = steering percentage left or right } @@ -148,7 +149,7 @@ int main(int argc, char **argv) talon2.SetInverted(false); talon3.SetNeutralMode(Brake); - double delay = 2.0; + double delay = 0.0; talon0.ConfigOpenloopRamp(delay,0); talon1.ConfigOpenloopRamp(delay,0); talon2.ConfigOpenloopRamp(delay,0); @@ -190,6 +191,7 @@ int main(int argc, char **argv) n->getParam(paramKey, vehicle); if (vehicle == "Simulator"){ simulator = true; + simulator = false; } double wheel[6]; //array to update motor values @@ -235,7 +237,7 @@ int main(int argc, char **argv) float talon_steer; if(hbeat){ - talon_speed = speed / 100.0; + talon_speed = speed / 50.0; talon_steer = steer / 100.0; } else{ @@ -253,13 +255,46 @@ int main(int argc, char **argv) } else if (speed<0){ talon_speed = -0.3; -}*/ - //talon_speed = speed/100; //1.5 +} PID STUFF*/ + + + float right = talon_speed - talon_steer; //Positive turn decreases right motors speeds to turn right. float left = talon_speed + talon_steer; - + + float delta_right = right - prev_right; + float delta_left = left - prev_left; + if (abs(right-0.0) < abs(prev_right-0.0) && abs(delta_right)>max_delta){ + if (delta_right > 0){ + right = prev_right + max_delta; + delta_right = max_delta; + } + else{ + right = prev_right - max_delta; + delta_right = max_delta; + } + } + + if (abs(left-0.0) < abs(prev_left-0.0) && abs(delta_left)>max_delta){ + if (delta_left > 0){ + left = prev_left + max_delta; + delta_left = max_delta; + } + else{ + left = prev_left - max_delta; + delta_left = max_delta; + } + } + prev_right = right; + prev_left = left; + + if(abs(right)>0.4){ + right = 0.0; + } + if(abs(left)>0.4){ + left = 0.0; + } //printf("%lf",talon_speed); - //LEFT SIDE talon0.Set(ControlMode::PercentOutput, left); @@ -270,11 +305,12 @@ int main(int argc, char **argv) talon4.Set(ControlMode::PercentOutput, right); talon5.Set(ControlMode::PercentOutput, right); - + //Output debug information - if (loopCount >= 10) { + if (loopCount >= 0) { loopCount = 0; //std::cout << "talon5 motor output: " << talon5.GetMotorOutputPercent() << std::endl; + // std::cout << "talon motor delta: " << delta_right << std::endl; // std::cout << "talon2 velocity: " << talon2.GetSelectedSensorVelocity() << std::endl; }