From cd6071da23a7d08308ca6697e39859750f4805fc Mon Sep 17 00:00:00 2001 From: scorpio1 Date: Mon, 25 Jul 2022 11:23:11 -0400 Subject: [PATCH] added better velocity control --- src/straight_line.py | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/straight_line.py b/src/straight_line.py index 33af3cf..a5f0a95 100755 --- a/src/straight_line.py +++ b/src/straight_line.py @@ -40,24 +40,31 @@ def convert_to_cylindrical(point): def get_error_acceleration(circle_position, r, velocity): #here we take the velocity and displacement in the direction of the second #and third unit vector, and use them to calculate accelerations - #based off modelling them as a damped spring + #based off modelling them as a damped spring or based off how far from + #the max speed they are velocity_unit_2 = np.dot(velocity, unit_vector_2) velocity_unit_3 = np.dot(velocity, unit_vector_3) circle_unit_2 = np.dot(circle_position, unit_vector_2) circle_unit_3 = np.dot(circle_position, unit_vector_3) - accel = -1*(b3*velocity_unit_2+k*circle_unit_2)*unit_vector_2 - accel = accel - (b*velocity_unit_3+k*circle_unit_3)*unit_vector_3 - return accel + accel_2_1 = -1*(b3*velocity_unit_2+k*circle_unit_2) + accel_2_2 = 2*(9.5-abs(velocity_unit_2))*velocity_unit_2/abs(velocity_unit_2) + + + accel_3_1 = -1*(b*velocity_unit_3+k*circle_unit_3) + accel_3_2 = 2*(9.5-abs(velocity_unit_3))*velocity_unit_3/abs(velocity_unit_3) + error_accel = min(velocity_unit_2*accel_2_1,velocity_unit_2*accel_2_2)*unit_vector_2/velocity_unit_2 + error_accel += min(velocity_unit_3*accel_3_1,velocity_unit_3*accel_3_2)*unit_vector_3/velocity_unit_3 + return error_accel def get_forward_acceleration(y, velocity): #here we calculate the forward acceleration, either based off how far off #the target speed we are, or based off a damped spring, depending on which #is less (or if we are close to the finish) forward_velocity=np.dot(velocity,unit_vector) - forward_acceleration = 2*(9.5-forward_velocity) + forward_acceleration = 2*(9.5-abs(forward_velocity))*forward_velocity/abs(forward_velocity) forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-100))/m - return min(forward_acceleration, forward_acceleration_2)*unit_vector + return min(forward_acceleration*forward_velocity, forward_acceleration_2*forward_velocity)*unit_vector/forward_velocity def guidance_law(): #here we calculate the acceleration we need in the forward direction, @@ -155,9 +162,9 @@ if __name__ == '__main__': #describe the vectors that define our direction of travel #the first vector is the direction we travel, the other two are two more #vectors that form an orthonormal basis with the first one - unit_vector = np.array([1,0,0]) - unit_vector_2 = np.array([0,0,-1]) - unit_vector_3 = np.array([0,1,0]) + unit_vector = np.array([1,5,0]) + unit_vector_2 = np.array([-5,1,0]) + unit_vector_3 = np.array([0,0,1]) unit_vector = unit_vector/np.linalg.norm(unit_vector) unit_vector_2 = unit_vector_2/np.linalg.norm(unit_vector_2) #here we start creating the subscribers, and launching the drone