From 8fc4c4b058c330982dd679e54bf0e5c81e5b824a Mon Sep 17 00:00:00 2001 From: Noah Klein Date: Thu, 18 Feb 2021 20:41:01 -0600 Subject: [PATCH 1/5] prepped to start running iterative turn tests --- flight/config.py | 3 +- flight/flight.py | 2 +- flight/utils/movement_controller.py | 73 ++++++++++++++++++++++++++++- run.py | 17 ++++--- speed15data.txt | 1 + speed5data.txt | 1 + 6 files changed, 86 insertions(+), 11 deletions(-) create mode 100644 speed15data.txt create mode 100644 speed5data.txt diff --git a/flight/config.py b/flight/config.py index 7d16c216..8879458b 100644 --- a/flight/config.py +++ b/flight/config.py @@ -4,7 +4,8 @@ from mavsdk import System -MAX_SPEED: float = 6.352 # m/s +# MAX_SPEED: float = 6.352 # m/s +MAX_SPEED: float = 5 # m/s ALT_CORRECTION_SPEED: float = 0.25 # m/s down MAX_ALT: float = 9.0 # m TAKEOFF_ALT: float = 1.0 # m diff --git a/flight/flight.py b/flight/flight.py index 3546766c..a0ac1fca 100644 --- a/flight/flight.py +++ b/flight/flight.py @@ -159,7 +159,7 @@ async def start_flight(comm, drone: System): # Worried about what happens here await asyncio.sleep(config.THINK_FOR_S) logging.info("Landing the drone") - await drone.action.land() + # await drone.action.land() except: logging.error("No system available") comm.set_state("final") diff --git a/flight/utils/movement_controller.py b/flight/utils/movement_controller.py index 2a6719e0..561b9e77 100644 --- a/flight/utils/movement_controller.py +++ b/flight/utils/movement_controller.py @@ -6,6 +6,10 @@ import logging import math +import time +import sys +import os + class MovementController: """ @@ -95,25 +99,90 @@ async def move_to(self, drone: System, pylon: LatLon) -> bool: return True count += 1 + f = 5 # initial forward vel + r = 0 # initial right vel + d = 0 # initial down vel + y = -30 # initial yawspeed + counter = 0 # initial count + tracker_time = time.time() + async def turn(self, drone: System) -> bool: """ Turns the drone around the pylon it is currently at Parameters: Drone(System): Our drone object """ + # counts the number of data points read/sent to the drone per turn count: int = 0 + + # Gets the velocity of the drone going into the turn + async for tel in drone.telemetry.position_velocity_ned(): + current_vel : double = (tel.velocity.north_m_s**2 + tel.velocity.east_m_s**2)**.5 + break + + MovementController.y = MovementController.y - 10 + if(MovementController.counter % 8 == 0): + MovementController.y = -30 + MovementController.d = MovementController.d - 1 + if(MovementController.counter % 40 == 0): + MovementController.d = 0 + MovementController.r = MovementController.r - 1 + if(MovementController.counter % 320 == 0): + MovementController.r = 0 + MovementController.f = MovementController.f + 5 + + MovementController.counter = MovementController.counter + 1 + + print("f:",MovementController.f,"r:",MovementController.r,"d:",MovementController.d,"y:",MovementController.y) + + MovementController.tracker_time = time.time() - MovementController.tracker_time + + # Making output to file data section ***************************** + + filename = "speed" + str(config.MAX_SPEED) + "data.txt" + + with open(filename, "a+") as outfile: + datastring = "Forward_vel: " + str(MovementController.f) + " Right_vel: " + str(MovementController.r) + " Down_vel: " + str(MovementController.d) + " Yawspeed: " + str(MovementController.y) + " Time: " + str(MovementController.tracker_time) + "\n" + outfile.write(datastring) + + # Making output to file data section ***************************** + + MovementController.tracker_time = time.time() + + # print("Current velocity: ", current_vel) + # Calculate the correct turning variables based off of the incoming speed + # forward_m_s = current_vel * (5/6) + # forward_m_s = current_vel + # right_m_s = current_vel * (-.5) + # yawspeed_d_s = current_vel * (-10) + async for tel in drone.telemetry.attitude_euler(): + # EulerAngle: [roll_deg, pitch_deg, yaw_deg] + # Calculates the current angle of the drone + # print(tel) current: float = (360 + round(tel.yaw_deg)) % 360 + # Calculate the angle required to turn 180 deg on the first data point if count == 0: temp = (current + 180) % 360 + # VelocityBodyYawspeed(forward m/s, right m/s, down m/s, yawspeed deg/s) + # Sends signal to the drone to turn. **No need to send this multiple times + # unless the value is changing each data point + # Original is VelocityBodyYawspeed(5, -3, -0.1, -60) await drone.offboard.set_velocity_body( - sdk.offboard.VelocityBodyYawspeed(5, -3, -0.1, -60) + # sdk.offboard.VelocityBodyYawspeed(5, -3, -0.1, -60) + # sdk.offboard.VelocityBodyYawspeed(forward_m_s, right_m_s, -0.1, -60) + sdk.offboard.VelocityBodyYawspeed(MovementController.f, + MovementController.r, + MovementController.d, + MovementController.y) ) # await asyncio.sleep(config.FAST_THINK_S) + # Finds the difference between the current angle and desired angle val = abs(current - temp) # TODO: Add case so that it can overshoot the point and still complete - if val < 10: + if val < 15: # originally 10, gave it more leeway + ############## loop iteration ############### logging.debug("Finished Turn") return True count += 1 diff --git a/run.py b/run.py index b8b1049f..4684725d 100755 --- a/run.py +++ b/run.py @@ -13,13 +13,16 @@ def main() -> None: - parser: argparse.ArgumentParser = argparse.ArgumentParser() - parser.add_argument( - "-s", "--simulation", help="using the simulator", action="store_true" - ) - args = parser.parse_args() - logging.debug("Simulation flag %s", "enabled" if args.simulation else "disabled") - run_threads(args.simulation) + try: + parser: argparse.ArgumentParser = argparse.ArgumentParser() + parser.add_argument( + "-s", "--simulation", help="using the simulator", action="store_true" + ) + args = parser.parse_args() + logging.debug("Simulation flag %s", "enabled" if args.simulation else "disabled") + run_threads(args.simulation) + except: + print("exception thrown all the way back to main") def init_flight(flight_args): diff --git a/speed15data.txt b/speed15data.txt new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/speed15data.txt @@ -0,0 +1 @@ + diff --git a/speed5data.txt b/speed5data.txt new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/speed5data.txt @@ -0,0 +1 @@ + From 41270813def78e8e0a89e4370d7854fdaef23ff0 Mon Sep 17 00:00:00 2001 From: Noah Klein Date: Thu, 25 Feb 2021 21:15:11 -0600 Subject: [PATCH 2/5] currently testing different turn coefficients --- flight/config.py | 4 +- flight/utils/movement_controller.py | 76 +++++++++++------------------ speed15data.txt | 1 - speed5data.txt | 1 - 4 files changed, 31 insertions(+), 51 deletions(-) delete mode 100644 speed15data.txt delete mode 100644 speed5data.txt diff --git a/flight/config.py b/flight/config.py index 8879458b..268aba22 100644 --- a/flight/config.py +++ b/flight/config.py @@ -5,7 +5,7 @@ # MAX_SPEED: float = 6.352 # m/s -MAX_SPEED: float = 5 # m/s +MAX_SPEED: float = 15 # m/s ALT_CORRECTION_SPEED: float = 0.25 # m/s down MAX_ALT: float = 9.0 # m TAKEOFF_ALT: float = 1.0 # m @@ -45,7 +45,7 @@ OFFSET: float = 0.005 # km DEG_OFFSET: int = 90 # deg -NUM_LAPS: int = 2 +NUM_LAPS: int = 960 # FOR TESTING TURN TIMES THINK_FOR_S: float = 2.0 FAST_THINK_S: float = 1.0 diff --git a/flight/utils/movement_controller.py b/flight/utils/movement_controller.py index 561b9e77..6b675402 100644 --- a/flight/utils/movement_controller.py +++ b/flight/utils/movement_controller.py @@ -6,10 +6,6 @@ import logging import math -import time -import sys -import os - class MovementController: """ @@ -99,12 +95,12 @@ async def move_to(self, drone: System, pylon: LatLon) -> bool: return True count += 1 - f = 5 # initial forward vel - r = 0 # initial right vel - d = 0 # initial down vel - y = -30 # initial yawspeed + # f = 5 # initial forward vel + # r = 0 # initial right vel + # d = 0 # initial down vel + # y = -30 # initial yawspeed counter = 0 # initial count - tracker_time = time.time() + # tracker_time = time.time() async def turn(self, drone: System) -> bool: """ @@ -120,62 +116,48 @@ async def turn(self, drone: System) -> bool: current_vel : double = (tel.velocity.north_m_s**2 + tel.velocity.east_m_s**2)**.5 break - MovementController.y = MovementController.y - 10 - if(MovementController.counter % 8 == 0): - MovementController.y = -30 - MovementController.d = MovementController.d - 1 - if(MovementController.counter % 40 == 0): - MovementController.d = 0 - MovementController.r = MovementController.r - 1 - if(MovementController.counter % 320 == 0): - MovementController.r = 0 - MovementController.f = MovementController.f + 5 - - MovementController.counter = MovementController.counter + 1 - - print("f:",MovementController.f,"r:",MovementController.r,"d:",MovementController.d,"y:",MovementController.y) - - MovementController.tracker_time = time.time() - MovementController.tracker_time + # if(MovementController.counter == 0): + # config.MAX_SPEED = 10 + # MovementController.counter += 1 + # else: + # config.MAX_SPEED += 1 + # + # print(config.MAX_SPEED) + # print(current_vel) - # Making output to file data section ***************************** + # Constant values + PI = 3.14159 + RADIUS = 30 + # TIME = 3.5 + # HALF_CIRCUMFERENCE = PI * RADIUS - filename = "speed" + str(config.MAX_SPEED) + "data.txt" + # radius = TIME * current_vel / PI - with open(filename, "a+") as outfile: - datastring = "Forward_vel: " + str(MovementController.f) + " Right_vel: " + str(MovementController.r) + " Down_vel: " + str(MovementController.d) + " Yawspeed: " + str(MovementController.y) + " Time: " + str(MovementController.tracker_time) + "\n" - outfile.write(datastring) + half_period = (PI * RADIUS) / current_vel - # Making output to file data section ***************************** - - MovementController.tracker_time = time.time() - - # print("Current velocity: ", current_vel) - # Calculate the correct turning variables based off of the incoming speed - # forward_m_s = current_vel * (5/6) - # forward_m_s = current_vel - # right_m_s = current_vel * (-.5) - # yawspeed_d_s = current_vel * (-10) + forward_m_s = current_vel + yawspeed_d_s = -180/half_period + # right_m_s = -(current_vel) / RADIUS + right_m_s = 0 + down_m_s = 0 async for tel in drone.telemetry.attitude_euler(): # EulerAngle: [roll_deg, pitch_deg, yaw_deg] # Calculates the current angle of the drone - # print(tel) current: float = (360 + round(tel.yaw_deg)) % 360 # Calculate the angle required to turn 180 deg on the first data point if count == 0: temp = (current + 180) % 360 + midpoint = (current + 90) % 360 + + print("Halfway", abs(midpoint - current)) # VelocityBodyYawspeed(forward m/s, right m/s, down m/s, yawspeed deg/s) # Sends signal to the drone to turn. **No need to send this multiple times # unless the value is changing each data point # Original is VelocityBodyYawspeed(5, -3, -0.1, -60) await drone.offboard.set_velocity_body( - # sdk.offboard.VelocityBodyYawspeed(5, -3, -0.1, -60) - # sdk.offboard.VelocityBodyYawspeed(forward_m_s, right_m_s, -0.1, -60) - sdk.offboard.VelocityBodyYawspeed(MovementController.f, - MovementController.r, - MovementController.d, - MovementController.y) + sdk.offboard.VelocityBodyYawspeed(forward_m_s, right_m_s, down_m_s, yawspeed_d_s) ) # await asyncio.sleep(config.FAST_THINK_S) # Finds the difference between the current angle and desired angle diff --git a/speed15data.txt b/speed15data.txt deleted file mode 100644 index 8b137891..00000000 --- a/speed15data.txt +++ /dev/null @@ -1 +0,0 @@ - diff --git a/speed5data.txt b/speed5data.txt deleted file mode 100644 index 8b137891..00000000 --- a/speed5data.txt +++ /dev/null @@ -1 +0,0 @@ - From c8a80761269efd9b5c1ed65fe4589729633625fa Mon Sep 17 00:00:00 2001 From: Noah Klein Date: Tue, 16 Mar 2021 20:27:37 -0500 Subject: [PATCH 3/5] finalized variable turn function --- flight/config.py | 5 ++-- flight/utils/movement_controller.py | 45 +++++++---------------------- 2 files changed, 13 insertions(+), 37 deletions(-) diff --git a/flight/config.py b/flight/config.py index 268aba22..2c30925f 100644 --- a/flight/config.py +++ b/flight/config.py @@ -4,8 +4,7 @@ from mavsdk import System -# MAX_SPEED: float = 6.352 # m/s -MAX_SPEED: float = 15 # m/s +MAX_SPEED: float = 6.352 # m/s ALT_CORRECTION_SPEED: float = 0.25 # m/s down MAX_ALT: float = 9.0 # m TAKEOFF_ALT: float = 1.0 # m @@ -45,7 +44,7 @@ OFFSET: float = 0.005 # km DEG_OFFSET: int = 90 # deg -NUM_LAPS: int = 960 # FOR TESTING TURN TIMES +NUM_LAPS: int = 2 # FOR TESTING TURN TIMES THINK_FOR_S: float = 2.0 FAST_THINK_S: float = 1.0 diff --git a/flight/utils/movement_controller.py b/flight/utils/movement_controller.py index 6b675402..af62f468 100644 --- a/flight/utils/movement_controller.py +++ b/flight/utils/movement_controller.py @@ -80,6 +80,7 @@ async def move_to(self, drone: System, pylon: LatLon) -> bool: * math.sin(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), y, ) + # continuously update information on the drone's location # and update the velocity of the drone await drone.offboard.set_velocity_ned( @@ -95,14 +96,7 @@ async def move_to(self, drone: System, pylon: LatLon) -> bool: return True count += 1 - # f = 5 # initial forward vel - # r = 0 # initial right vel - # d = 0 # initial down vel - # y = -30 # initial yawspeed - counter = 0 # initial count - # tracker_time = time.time() - - async def turn(self, drone: System) -> bool: + async def turn(self, drone: System, degrees_turned = 180: int, left_turn = True: bool, time = 3.5: float) -> bool: """ Turns the drone around the pylon it is currently at Parameters: @@ -116,28 +110,10 @@ async def turn(self, drone: System) -> bool: current_vel : double = (tel.velocity.north_m_s**2 + tel.velocity.east_m_s**2)**.5 break - # if(MovementController.counter == 0): - # config.MAX_SPEED = 10 - # MovementController.counter += 1 - # else: - # config.MAX_SPEED += 1 - # - # print(config.MAX_SPEED) - # print(current_vel) - - # Constant values - PI = 3.14159 - RADIUS = 30 - # TIME = 3.5 - # HALF_CIRCUMFERENCE = PI * RADIUS - - # radius = TIME * current_vel / PI + # Constant time + yawspeed_d_s = ((-1 * left_turn) * degrees_turned)/time - half_period = (PI * RADIUS) / current_vel - - forward_m_s = current_vel - yawspeed_d_s = -180/half_period - # right_m_s = -(current_vel) / RADIUS + forward_m_s = 0 right_m_s = 0 down_m_s = 0 @@ -147,21 +123,22 @@ async def turn(self, drone: System) -> bool: current: float = (360 + round(tel.yaw_deg)) % 360 # Calculate the angle required to turn 180 deg on the first data point if count == 0: - temp = (current + 180) % 360 - midpoint = (current + 90) % 360 - - print("Halfway", abs(midpoint - current)) + temp = (current + ((-1 * left_turn) * degrees_turned)) % 360 + midpoint = (current + ((-1 * left_turn) * degrees_turned / 2)) % 360 # VelocityBodyYawspeed(forward m/s, right m/s, down m/s, yawspeed deg/s) # Sends signal to the drone to turn. **No need to send this multiple times # unless the value is changing each data point - # Original is VelocityBodyYawspeed(5, -3, -0.1, -60) await drone.offboard.set_velocity_body( sdk.offboard.VelocityBodyYawspeed(forward_m_s, right_m_s, down_m_s, yawspeed_d_s) ) # await asyncio.sleep(config.FAST_THINK_S) # Finds the difference between the current angle and desired angle val = abs(current - temp) + mid_val = abs(current - midpoint) + + if mid_val < 15: + forward_m_s = current_vel # TODO: Add case so that it can overshoot the point and still complete if val < 15: # originally 10, gave it more leeway ############## loop iteration ############### From 2596a310030b752da872c2ba7362eab03d4ee7b6 Mon Sep 17 00:00:00 2001 From: pieperm Date: Sat, 20 Mar 2021 02:45:59 -0500 Subject: [PATCH 4/5] Adjust type annotations, revert some changes made for testing purposes --- flight/flight.py | 2 +- flight/utils/movement_controller.py | 25 ++++++++++++++----------- run.py | 17 +++++++---------- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/flight/flight.py b/flight/flight.py index a0ac1fca..3546766c 100644 --- a/flight/flight.py +++ b/flight/flight.py @@ -159,7 +159,7 @@ async def start_flight(comm, drone: System): # Worried about what happens here await asyncio.sleep(config.THINK_FOR_S) logging.info("Landing the drone") - # await drone.action.land() + await drone.action.land() except: logging.error("No system available") comm.set_state("final") diff --git a/flight/utils/movement_controller.py b/flight/utils/movement_controller.py index af62f468..cf965711 100644 --- a/flight/utils/movement_controller.py +++ b/flight/utils/movement_controller.py @@ -96,26 +96,29 @@ async def move_to(self, drone: System, pylon: LatLon) -> bool: return True count += 1 - async def turn(self, drone: System, degrees_turned = 180: int, left_turn = True: bool, time = 3.5: float) -> bool: + async def turn(self, drone: System, degrees_turned: int = 180, left_turn: bool = True, time: float = 3.5) -> bool: """ Turns the drone around the pylon it is currently at Parameters: - Drone(System): Our drone object + drone(System): Our drone object + degrees_turned(int): The number of degrees to turn + left_turn(bool): Direction of the turn; left if True, right if False + time(float): Amount of time to complete the turn """ # counts the number of data points read/sent to the drone per turn count: int = 0 # Gets the velocity of the drone going into the turn async for tel in drone.telemetry.position_velocity_ned(): - current_vel : double = (tel.velocity.north_m_s**2 + tel.velocity.east_m_s**2)**.5 + current_vel: float = (tel.velocity.north_m_s**2 + tel.velocity.east_m_s**2)**.5 break # Constant time - yawspeed_d_s = ((-1 * left_turn) * degrees_turned)/time + yawspeed_d_s: float = ((-1 * left_turn) * degrees_turned)/time - forward_m_s = 0 - right_m_s = 0 - down_m_s = 0 + forward_m_s: float = 0 + right_m_s: float = 0 + down_m_s: float = 0 async for tel in drone.telemetry.attitude_euler(): # EulerAngle: [roll_deg, pitch_deg, yaw_deg] @@ -123,8 +126,8 @@ async def turn(self, drone: System, degrees_turned = 180: int, left_turn = True: current: float = (360 + round(tel.yaw_deg)) % 360 # Calculate the angle required to turn 180 deg on the first data point if count == 0: - temp = (current + ((-1 * left_turn) * degrees_turned)) % 360 - midpoint = (current + ((-1 * left_turn) * degrees_turned / 2)) % 360 + temp: float = (current + ((-1 * left_turn) * degrees_turned)) % 360 + midpoint: float = (current + ((-1 * left_turn) * degrees_turned / 2)) % 360 # VelocityBodyYawspeed(forward m/s, right m/s, down m/s, yawspeed deg/s) # Sends signal to the drone to turn. **No need to send this multiple times @@ -134,8 +137,8 @@ async def turn(self, drone: System, degrees_turned = 180: int, left_turn = True: ) # await asyncio.sleep(config.FAST_THINK_S) # Finds the difference between the current angle and desired angle - val = abs(current - temp) - mid_val = abs(current - midpoint) + val: float = abs(current - temp) + mid_val: float = abs(current - midpoint) if mid_val < 15: forward_m_s = current_vel diff --git a/run.py b/run.py index 4684725d..b8b1049f 100755 --- a/run.py +++ b/run.py @@ -13,16 +13,13 @@ def main() -> None: - try: - parser: argparse.ArgumentParser = argparse.ArgumentParser() - parser.add_argument( - "-s", "--simulation", help="using the simulator", action="store_true" - ) - args = parser.parse_args() - logging.debug("Simulation flag %s", "enabled" if args.simulation else "disabled") - run_threads(args.simulation) - except: - print("exception thrown all the way back to main") + parser: argparse.ArgumentParser = argparse.ArgumentParser() + parser.add_argument( + "-s", "--simulation", help="using the simulator", action="store_true" + ) + args = parser.parse_args() + logging.debug("Simulation flag %s", "enabled" if args.simulation else "disabled") + run_threads(args.simulation) def init_flight(flight_args): From 4fb9bc97562f330b07ad24044e87545e56ec6726 Mon Sep 17 00:00:00 2001 From: pieperm Date: Sat, 20 Mar 2021 02:47:55 -0500 Subject: [PATCH 5/5] Pre-push formatting --- flight/utils/movement_controller.py | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/flight/utils/movement_controller.py b/flight/utils/movement_controller.py index cf965711..d33b127d 100644 --- a/flight/utils/movement_controller.py +++ b/flight/utils/movement_controller.py @@ -96,7 +96,13 @@ async def move_to(self, drone: System, pylon: LatLon) -> bool: return True count += 1 - async def turn(self, drone: System, degrees_turned: int = 180, left_turn: bool = True, time: float = 3.5) -> bool: + async def turn( + self, + drone: System, + degrees_turned: int = 180, + left_turn: bool = True, + time: float = 3.5, + ) -> bool: """ Turns the drone around the pylon it is currently at Parameters: @@ -110,11 +116,13 @@ async def turn(self, drone: System, degrees_turned: int = 180, left_turn: bool = # Gets the velocity of the drone going into the turn async for tel in drone.telemetry.position_velocity_ned(): - current_vel: float = (tel.velocity.north_m_s**2 + tel.velocity.east_m_s**2)**.5 + current_vel: float = ( + tel.velocity.north_m_s ** 2 + tel.velocity.east_m_s ** 2 + ) ** 0.5 break # Constant time - yawspeed_d_s: float = ((-1 * left_turn) * degrees_turned)/time + yawspeed_d_s: float = ((-1 * left_turn) * degrees_turned) / time forward_m_s: float = 0 right_m_s: float = 0 @@ -127,13 +135,17 @@ async def turn(self, drone: System, degrees_turned: int = 180, left_turn: bool = # Calculate the angle required to turn 180 deg on the first data point if count == 0: temp: float = (current + ((-1 * left_turn) * degrees_turned)) % 360 - midpoint: float = (current + ((-1 * left_turn) * degrees_turned / 2)) % 360 + midpoint: float = ( + current + ((-1 * left_turn) * degrees_turned / 2) + ) % 360 # VelocityBodyYawspeed(forward m/s, right m/s, down m/s, yawspeed deg/s) # Sends signal to the drone to turn. **No need to send this multiple times # unless the value is changing each data point await drone.offboard.set_velocity_body( - sdk.offboard.VelocityBodyYawspeed(forward_m_s, right_m_s, down_m_s, yawspeed_d_s) + sdk.offboard.VelocityBodyYawspeed( + forward_m_s, right_m_s, down_m_s, yawspeed_d_s + ) ) # await asyncio.sleep(config.FAST_THINK_S) # Finds the difference between the current angle and desired angle @@ -143,7 +155,7 @@ async def turn(self, drone: System, degrees_turned: int = 180, left_turn: bool = if mid_val < 15: forward_m_s = current_vel # TODO: Add case so that it can overshoot the point and still complete - if val < 15: # originally 10, gave it more leeway + if val < 15: # originally 10, gave it more leeway ############## loop iteration ############### logging.debug("Finished Turn") return True