scorpio1
/
OscillationControlWithCircle
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

added better velocity control

This commit is contained in:
scorpio1 2022-07-25 11:23:11 -04:00
parent ff991fe06d
commit cd6071da23
1 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/straight_line.py
View File

@ -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