diff --git a/src/straight_line.py b/src/straight_line.py index 2b36d70..33af3cf 100755 --- a/src/straight_line.py +++ b/src/straight_line.py @@ -74,8 +74,7 @@ def guidance_law(): accelerations=Vector3Stamped() accelerations.vector.x = accel[0] accelerations.vector.y=accel[1] - #we add 2 to the vertical acceleration because otherwise it sometimes - #just falls + accelerations.vector.z=accel[2] return accelerations @@ -150,15 +149,15 @@ if __name__ == '__main__': k = 2 k2 = 2 m=1.5 - b3 = 2*np.sqrt(m*k) - b = 2*np.sqrt(m*k) - b2 = 2*np.sqrt(m*k2) + b3 = 0.4*np.sqrt(m*k) + b = 0.4*np.sqrt(m*k) + b2 = 0.4*np.sqrt(m*k2) #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,1,0]) - unit_vector_3 = np.array([0,0,1]) + unit_vector_2 = np.array([0,0,-1]) + unit_vector_3 = np.array([0,1,0]) 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