diff --git a/Python3/src/cognitiveModel.py b/Python3/src/cognitiveModel.py index 9b1ecb7..f442e0d 100644 --- a/Python3/src/cognitiveModel.py +++ b/Python3/src/cognitiveModel.py @@ -4,6 +4,7 @@ import xpc import math # import geopy from geographiclib.geodesic import Geodesic as geo +from rich import print @@ -12,6 +13,7 @@ class scaleFactor(): SCALEYOKEPULL = 10 SCALEYOKESTEER = 10 SCALERUDDER = 10 + SCALELATITUDERUDDER = 0.02 SCALETHROTTLE = 1000 ###Define variables/parameters for aircraft class/category : Wisdom of Raju @@ -22,13 +24,15 @@ class AircraftLandingModel(pyactr.ACTRModel): self.inProgress = True self.printControlsFlag = printFlag self.targetLat = 39.895791 - self.targetLong = -104.696085 + self.targetLong = -104.696014 """ Setting DREF variables and loading into drefs array """ airspeedDREF = "sim/cockpit2/gauges/indicators/airspeed_kts_pilot" rollDREF = "sim/cockpit2/gauges/indicators/roll_AHARS_deg_pilot" magneticHeadingDREF = "sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_pilot" + latitudeDREF = "sim/flightmodel/position/latitude" ## Lat + longitudeDREF = "sim/flightmodel/position/longitude" ##Long verticalSpeedDREF = "sim/flightmodel/position/vh_ind_fpm" altitudeAGLDREF = "sim/flightmodel/position/y_agl" pitchDREF = "sim/flightmodel/position/true_theta" @@ -41,6 +45,8 @@ class AircraftLandingModel(pyactr.ACTRModel): "airspeed" : airspeedDREF, "roll" : rollDREF, "heading" : magneticHeadingDREF, + "latitude": latitudeDREF, + "longitude": longitudeDREF, "vertical speed" : verticalSpeedDREF, "altitude": altitudeAGLDREF, "pitch" : pitchDREF, @@ -55,6 +61,8 @@ class AircraftLandingModel(pyactr.ACTRModel): airspeed = self.client.getDREF("sim/cockpit2/gauges/indicators/airspeed_kts_pilot") roll = self.client.getDREF("sim/cockpit2/gauges/indicators/roll_AHARS_deg_pilot") heading = self.client.getDREF("sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_pilot") + latitude = self.client.getDREF("sim/flightmodel/position/latitude") ##Current Lat + longitude = self.client.getDREF("sim/flightmodel/position/longitude") ##Current Long descent_rate = self.client.getDREF("sim/flightmodel/position/vh_ind_fpm") altitude = self.client.getDREF("sim/flightmodel/position/y_agl") pitch = self.client.getDREF("sim/flightmodel/position/true_theta") @@ -67,6 +75,8 @@ class AircraftLandingModel(pyactr.ACTRModel): self.airspeed = airspeed[0] self.roll = roll[0] self.heading = HARDCODE_HEADING + self.latitude = latitude[0] + self.longitude = longitude[0] self.descent_rate = descent_rate[0] self.altitude = altitude[0] self.pitch = pitch[0] @@ -79,6 +89,8 @@ class AircraftLandingModel(pyactr.ACTRModel): "airspeed" : self.airspeed, "roll" : self.roll, "heading" : self.heading, + "latitude": self.latitude, + "longitude": self.longitude, "vertical speed" : self.descent_rate, "altitude": self.altitude, "pitch" : self.pitch, @@ -93,10 +105,12 @@ class AircraftLandingModel(pyactr.ACTRModel): self.target_airspeed = 80 self.target_roll = 0 self.target_heading = self.heading #Track heading from initialization[DEPRECATED] + self.target_Lat = self.latitude #Track Lat + self.target_Long = self.longitude #Track Long self.target_descent_rate = 500 self.target_altitude = -998 self.target_pitch = 20 - self.targets = [self.target_airspeed,self.target_roll,self.target_heading,self.target_descent_rate,self.target_altitude,self.target_pitch] + self.targets = [self.target_airspeed,self.target_roll,self.target_heading,self.target_Lat,self.target_Long,self.target_descent_rate,self.target_altitude,self.target_pitch] #State Flags (Boolean) & Current State (Integer) @@ -122,6 +136,8 @@ class AircraftLandingModel(pyactr.ACTRModel): self.integral_airspeed = 0 self.integral_roll = 0 self.integral_heading = 0 + self.integral_Latitude = 0 + self.integral_Longitude = 0 self.integral_descent_rate = 0 #Flare Specific Parameters @@ -159,7 +175,7 @@ class AircraftLandingModel(pyactr.ACTRModel): def get_bearing(self,lat1, lat2, long1, long2): brng = geo.WGS84.Inverse(lat1, long1, lat2, long2)['azi1'] self.target_heading = brng - print("TARGET BEARING: " + str(brng)) + # print("TARGET BEARING: " + str(brng)) def getAndLoadDREFS(self): results = self.client.getDREFs(self.sources.values()) @@ -171,6 +187,7 @@ class AircraftLandingModel(pyactr.ACTRModel): lat = self.client.getDREF("sim/flightmodel/position/latitude") ##Current Lat long = self.client.getDREF("sim/flightmodel/position/longitude") ##Current Long self.get_bearing(lat[0],self.targetLat,long[0],self.targetLong) + # print("getDrefs: " + str(results[1][0])) # print("current destination: " + str(self.destinations["airspeed"])) @@ -193,11 +210,36 @@ class AircraftLandingModel(pyactr.ACTRModel): # print("*Parameter,Target,Current,Yoke Steer: " + "Roll, " + str(self.target_roll) + "," + str(self.destinations["roll"])+ "," + str(yokeSteer)) # print("*Parameter,Target,Current,Rudder: " + "Heading, " + str(self.target_heading) + "," + str(self.heading)+ "," + str(rudder)) # print("*Parameter,Target,Current,Throttle: " + "Descent Rate, " + str(self.target_descent_rate) + "," + str(self.descent_rate)+ "," + str(throttle)) - parameter = ["Airspeed","Roll","Heading","Descent Rate","Altitude","Flare: Pitch", "Brakes: Wheel Speed", "Brakes: Wheel Weight"] - target = [str(round(self.target_airspeed)),str(round(self.target_roll)),str(round(self.target_heading,3)),str(round(self.target_descent_rate,3)),str(round(self.altitude,3)),str(round(self.target_pitch,3)),0, 0] - current = [str(round(self.dictionaryAccess(self.destinations,"pitch"),3)),str(round(self.dictionaryAccess(self.destinations,"roll"),3)),str(round(self.dictionaryAccess(self.destinations,"heading"),3)),str(round(self.dictionaryAccess(self.destinations,"vertical speed"),3)), - str(round(self.dictionaryAccess(self.destinations,"altitude"),3)),str(round(self.dictionaryAccess(self.destinations,"pitch"),3)),str(round(self.dictionaryAccess(self.destinations,"wheelSpeed"),3)),str(round(self.dictionaryAccess(self.destinations,"wheelWeight"),3))] - controlVal = [str(round(yokePull,3)),str(round(yokeSteer,3)),str(round(rudder,3)),str(round(throttle,3)),str(round(self.altitude,3)),str(self.dictionaryAccess(self.phaseFlags,"flare")),str(round(self.dictionaryAccess(self.destinations,"brakes"),3)),str(round(self.dictionaryAccess(self.destinations,"brakes"),3))] + parameter = ["Airspeed","Roll","Heading","Longitude","Descent Rate","Altitude","Flare: Pitch", "Brakes: Wheel Speed", "Brakes: Wheel Weight"] + target = [str(round(self.target_airspeed)), + str(round(self.target_roll)), + str(round(self.target_heading,3)), + str(round(self.target_Long,6)), + str(round(self.target_descent_rate,3)), + str(round(self.altitude,3)), + str(round(self.target_pitch,3)), + 0, + 0] + + current = [str(round(self.dictionaryAccess(self.destinations,"pitch"),3)), + str(round(self.dictionaryAccess(self.destinations,"roll"),3)), + str(round(self.dictionaryAccess(self.destinations,"heading"),3)), + str(round(self.dictionaryAccess(self.destinations,"longitude"),6)), + + str(round(self.dictionaryAccess(self.destinations,"vertical speed"),3)), + str(round(self.dictionaryAccess(self.destinations,"altitude"),3)), + str(round(self.dictionaryAccess(self.destinations,"pitch"),3)), + str(round(self.dictionaryAccess(self.destinations,"wheelSpeed"),3)), + str(round(self.dictionaryAccess(self.destinations,"wheelWeight"),3))] + controlVal = [str(round(yokePull,3)), + str(round(yokeSteer,3)), + str(round(rudder,3)), + str(round(rudder,3)), + str(round(throttle,3)), + str(round(self.altitude,3)), + str(self.dictionaryAccess(self.phaseFlags,"flare")), + str(round(self.dictionaryAccess(self.destinations,"brakes"),3)), + str(round(self.dictionaryAccess(self.destinations,"brakes"),3))] header_row = "{:<20} {:<20} {:<20} {:>10}" headers = "Parameter Target Current Control_Value".split() @@ -281,7 +323,18 @@ class AircraftLandingModel(pyactr.ACTRModel): # scaleFactor.SCALEYOKEPULL) yoke_steer, self.integral_roll = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"roll"), self.target_roll, self.integral_roll,scaleFactor.SCALEYOKESTEER) - rudder, self.integral_heading = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"heading"), self.target_heading, self.integral_heading,scaleFactor.SCALERUDDER) + + """ + RUDDER CONTROLS + """ + #ORIGINAL + # rudder, self.integral_heading = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"heading"), self.target_heading, self.integral_heading,scaleFactor.SCALERUDDER) + #lATITUDE/LONGITUDE + rudder, self.integral_Longitude = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"longitude"), self.target_Long, self.integral_Longitude,scaleFactor.SCALELATITUDERUDDER) + + + + throttle, self.integral_descent_rate = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"vertical speed"), self.target_descent_rate, self.integral_descent_rate,scaleFactor.SCALETHROTTLE) ### 1. For Calculated Yoke and Throttle Values #Invert Throttle Control & divide by 5 to scale @@ -323,7 +376,7 @@ class AircraftLandingModel(pyactr.ACTRModel): ##Method 2: Same Control Statements with Change in Parameter to decided pitch from Airspeed ---> Local Pitch Relative to the Horizon - + rudder = rudder * -1 #Switch Target for Pitch to Local Pitch Axis (ex. +10 Degrees nose up) if(self.printControlsFlag): self.printControls(1,0,yoke_pull,yoke_steer,rudder,throttle) #PRINT CONTROLS @@ -383,7 +436,7 @@ class AircraftLandingModel(pyactr.ACTRModel): and self.dictionaryAccess(self.destinations,"brakes") == 1): self.inProgress = False - def simulationStatus(self): + def getSimulationStatus(self): return self.inProgress diff --git a/Python3/src/experiments/weather_files/weather.csv b/Python3/src/experiments/weather_files/weather.csv new file mode 100644 index 0000000..5c96b08 --- /dev/null +++ b/Python3/src/experiments/weather_files/weather.csv @@ -0,0 +1,8 @@ +Experiment Number,Starting Altitude (AGL),Wind Layer Altitude,Wind Direction,Wind Speed +1,2000,0,179,0 +2,2000,0,179,5 +3,2000,0,179,10 +4,2000,0,179,15 +5,2000,0,179,20 +6,2000,0,179,25 +7,2000,0,179,30 \ No newline at end of file diff --git a/Python3/src/experiments/weather_files/weather_2.csv b/Python3/src/experiments/weather_files/weather_2.csv deleted file mode 100644 index 113ec39..0000000 --- a/Python3/src/experiments/weather_files/weather_2.csv +++ /dev/null @@ -1,5 +0,0 @@ -Wind,2,3,4 -Altitudes,3,4,5 -Turtles,4,5,6 -Ducks,5,6,7 -Penguins,6,7,8 \ No newline at end of file diff --git a/Python3/src/testPlatform.py b/Python3/src/testPlatform.py index 6c19fcf..63c1aeb 100644 --- a/Python3/src/testPlatform.py +++ b/Python3/src/testPlatform.py @@ -1,5 +1,9 @@ + + import datetime from math import cos, pi, sin, sqrt + + import os import time from time import sleep @@ -7,8 +11,12 @@ import xpc from cognitiveModel import AircraftLandingModel import shutil import csv +from enum import Enum +class messageType(Enum): + REGULAR = 1 + ERROR = 2 def eulerToQuat(psiInput,thetaInput,phiInput): @@ -24,28 +32,25 @@ def eulerToQuat(psiInput,thetaInput,phiInput): quat = [q0/e,q1/e,q2/e,q3/e] return quat - -def loadFile(index): - filename = "Python3/src/experiments/weather_files/weather_" + str(index) + ".csv" +def loadFile(): + filename = "Python3/src/experiments/weather_files/weather.csv" with open(filename,"r") as f: matrix = list(csv.reader(f,delimiter=',')) print(matrix) return matrix + def selectWeather(matrix,experimentNumber): return matrix[experimentNumber] - - -def experimentSetUp(client,altitudeInput,newExperiment): - loadFile(2) +def experimentSetUp(client,currentConditions,newExperiment): # input("Check The Loaded File Now") print("Entered: EXPERIMENTSETUP") if(newExperiment): #Location: groundLevel = 5434 - offset = altitudeInput + offset = float(currentConditions[1]) altitudeFEET = groundLevel + offset altitudeMETERS = altitudeFEET/3.281 altitude = altitudeMETERS @@ -58,6 +63,7 @@ def experimentSetUp(client,altitudeInput,newExperiment): client.sendDATA(data) + print("Zero velocities") x = "sim/flightmodel/position/local_vx" y = "sim/flightmodel/position/local_vy" @@ -104,19 +110,18 @@ def experimentSetUp(client,altitudeInput,newExperiment): # result = client.getDREF(windLayer) # print("Wind:" + str(result)) print("Setting Wind Layers") - client.sendDREF(windLayer,6000) - client.sendDREF(windLayer2,15000) - client.sendDREF(windLayer3,15000) + client.sendDREF(windLayer,float(currentConditions[2])) + # client.sendDREF(windLayer2,15000) + # client.sendDREF(windLayer3,15000) print("Setting Wind Direction and Speed") - client.sendDREF(windDirection,170) - print("Set 1") - client.sendDREF(windSpeed,40) + client.sendDREF(windDirection,float(currentConditions[3])) + print("Set 1") + client.sendDREF(windSpeed,float(currentConditions[4])) print("Set 2") # client.sendDREF(turbulence,turbulencePercentage) - # preset = "sim/weather/region/weather_preset" # value = 8 # client.sendDREF(preset,value) @@ -139,121 +144,133 @@ def experimentSetUp(client,altitudeInput,newExperiment): 18 - Angle of Attack 20 - Latitude and Longitude """ - kias = -998 - keas = -998 - ktas = -998 - ktgs = -998 - mph = -998 - mphas = 80 - mphgs = 80 - - print("Setting orientation for test") - # client.sendDREF("sim/operation/override/override_planepath",1) - # client.pauseSim(True) - - # client.sendDREF("sim/flightmodel/position/local_y",2000) - # client.sendDREFs(["sim/flightmodel/position/P","sim/flightmodel/position/Q","sim/flightmodel/position/R"],[0,0,0]) - # client.sendDREF("sim/flightmodel/position/q",[0.3, 0, 0, 0.6]) - - # data = [\ - # [3, -998, -998, -998, -998, -998, -998, mphas, mphgs],\ - # [16, 0, 0, 0, -998, -998, -998, -998, -998],\ - # [17, 0, 0, 0, -998, -998, -998, -998, -998],\ - # [18, -998, -998, -998, -998, -998, -998, -998, -998],\ - # [19, -998, -998, -998, -998, -998, -998, -998, -998],\ - # testLocation\ - # ] - # client.sendDATA(data) - # client.sendDREF("sim/operation/override/override_planepath",0) - - # Set control surfaces and throttle of the player aircraft using sendCTRL - # print("Setting controls") - # ctrl = [0.0, 0.0, 0.0, 0.0] - # cogModel.client.sendCTRL(ctrl) - # print("past setting controls") + message = "Conditions are set as\n" \ + "Starting Altitude: {} \n" \ + "Layer Altitude: {} \n" \ + "Wind Direction: {} \n" \ + "Wind Speed: {} \n".format(currentConditions[1],currentConditions[2],currentConditions[3],currentConditions[4]) + specialPrint(message,False,messageType.REGULAR) else: print("Experiment currently in progress, not resetting position and environmental conditions") -def runExperiment(title,printFlag,experimentStart): - print("Model Test: Xplane setting up connection") - print("Setting up simulation") +def runExperiment(title,currentConditions,allowPrinting,isNewExperiment): + specialPrint("New Experiment\nSetting Up the Simulation",False,messageType.REGULAR) startTime = 0 endTime = 0 - difference = endTime - startTime - experimentLive = True + timeElapsed = endTime - startTime + experimentInProgress = True timeoutLimit = 10 - newExperiment = experimentStart - while(difference < timeoutLimit and experimentLive): - print("Time Elapsed: -----> " + str(difference)) + newExperiment = isNewExperiment + + while(timeElapsed <= timeoutLimit and experimentInProgress): + # print("Time Elapsed: -----> " + str(timeElapsed)) try: with xpc.XPlaneConnect() as client: - # Verify connection + """ + Test Connection + """ client.getDREF("sim/test/test_float") - cogModel = AircraftLandingModel(client,printFlag) - experimentSetUp(cogModel.client,2000,newExperiment) + """ + Setup Model + """ + cogModel = AircraftLandingModel(client,allowPrinting) + """ + Set Simulator Conditions + """ + experimentSetUp(cogModel.client,currentConditions,newExperiment) + """ + """ cogModel.client.pauseSim(False) - count = 0 - innercount = 0 - clockStart = time.time() - retry = 0 newExperiment = False - while(cogModel.simulationStatus()): - clockStart = time.time() #START TIMER - #Run Model - cogModel.update_aircraft_state() + + """ + Single Experiment Loop + """ + while(experimentInProgress): + cogModel.update_aircraft_state() cogModel.update_controls_simultaneously() - client.pauseSim(False) #Unpause Simulator - clockEnd = time.time() # STOP TIMER - count+=1 - # print("Clock Time: " + str(clockEnd - clockStart)) ## LOG TIME TAKEN - sleep(0.05) # LET Simulator Run 50 Milliseconds + client.pauseSim(False) #Unpause Simulator + sleep(0.05) # Let Simulator Run 50 Milliseconds startTime = time.time() - experimentLive = cogModel.simulationStatus() + experimentInProgress = cogModel.getSimulationStatus() except: - print("except detected") endTime = time.time() - print("Start Time:" + str(startTime)) - print("End Time:" + str(endTime)) - difference = endTime - startTime + timeElapsed = endTime - startTime + message = "Except detected\nStart Time: {a}\nEnd Time: {b}\n" \ + "Time Elapsed: -----> {c} ".format(a= startTime, b=endTime,c=timeElapsed) + specialPrint(message,False,messageType.ERROR) continue - if(difference >= timeoutLimit): - print("Timeout[" + str(difference) +"]:"+"Error, please run test again") + + """ + Parse End Condition: Succesful Run or Timeout-Induced End + """ + if(timeElapsed >= timeoutLimit): + print("Timeout[" + str(timeElapsed) +"]:"+"Error, please run test again") else: print("Model has finished running") - #Copy data.txt to the cloudddddd using python magic and accurate filepaths + + """ + Ask Experimenter if they would like to exit experiment battery early + """ + exitDecision = input("Press 'y' or any key to continue, press 'n' to exit...") + if(exitDecision == "n"): + exitExperimentLoop = True + else: + exitExperimentLoop = False + return exitExperimentLoop + +def setUp(): + open("/Users/flyingtopher/X-Plane 11/Data.txt", 'w') + specialPrint("Data Collection File Ready",False,messageType.REGULAR) + +def cleanUp(title): now = datetime.datetime shutil.copy("/Users/flyingtopher/X-Plane 11/Data.txt", "/Users/flyingtopher/Desktop/Test Destination/" + title + "_" + str(now.now()) + "_" + ".txt") - # print("CLEAN UP: Data File Copied and saved") - # os.remove("/Users/flyingtopher/X-Plane 11/Data.txt") print("CLEAN UP: Data File Deleted and Reset") - # exit = input("Press 'y' or any key to continue, press 'n' to exit...") - exit = False - if(exit == "n"): - exit = True - else: - exit = False - return exit - ##Reset the sim with the keyboard shortcut (wrapper around model that waits for reconnection) - -def ex(): - ##Store Experiment Battery - ##Different paramters on every run of the model - ## Nested loops: - ##wind conditions, pilot conditions - # title = input("Please Enter Experiment Set Title, leave blank for trial runs") - title = 'test' - count = 0 - while(count<7): - # input("Press Enter to Start Experiment #" + str(count) + ": ") - print("Data File Reset") - f = open("/Users/flyingtopher/X-Plane 11/Data.txt", 'w') - exit = runExperiment(title,False,True) - if(exit): - break - count+=1 - print("Experiment Battery Complete") + specialPrint("Data File Ready",False,messageType.REGULAR) + +def specialPrint(text, inputRequested,type): + if(type == messageType.REGULAR): + print("-" * 81) + print(text) + print("-" * 81 + "\n") + if(inputRequested): + inputReceived = input() + return inputReceived + if(type == messageType.ERROR): + print("*" * 81) + print(text) + print("*" * 81 + "\n") + + +def ex(): + """ + One Time experimental setup + """ + title = specialPrint("Please Enter Experiment Set Title, leave blank for trial runs\n", True, messageType.REGULAR) + experimentConditionMatrix = loadFile() + allowPrinting = True + isNewExperiment = True + experimentCount = 0 + """ + Experiment Loop + """ + while(experimentCount