diff --git a/src/obstacle_detection.py b/src/obstacle_detection.py index 8a0487f..404dfba 100755 --- a/src/obstacle_detection.py +++ b/src/obstacle_detection.py @@ -66,9 +66,9 @@ def avoidance(weights): if velocity[0]<0: angle_velocity+=np.pi if go_left: - angle_acceleration = angle_velocity+np.pi/2 - else: angle_acceleration = angle_velocity-np.pi/2 + else: + angle_acceleration = angle_velocity+np.pi/2 magnitude_acceleration = 1.2*(velocity[0]*velocity[0]+velocity[1]*velocity[1])/min_radius acceleration_x = np.cos(angle_acceleration)*magnitude_acceleration acceleration_y = np.sin(angle_acceleration)*magnitude_acceleration @@ -113,7 +113,7 @@ def get_forward_acceleration(y, velocity): #is less (or if we are close to the finish) forward_velocity=np.dot(velocity,unit_vector) forward_acceleration = 2*(9.5-abs(forward_velocity))*forward_velocity/abs(forward_velocity) - forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-100))/m + forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-1000))/m return min(forward_acceleration*forward_velocity, forward_acceleration_2*forward_velocity)*unit_vector/forward_velocity def guidance_law(): @@ -124,6 +124,10 @@ def guidance_law(): error_accel = get_error_acceleration(circle_position,r,velocity) #we then add these together to get our acceleration vector accel_straight = forward_accel+error_accel + magnitude_accel=np.linalg.norm(accel_straight) + if magnitude_accel>2*G: + accel_straight=accel_straight/magnitude_accel*2*G + #next we deal with the avoidance component balance = get_balance(weights) avoidance_accel = avoidance(weights) @@ -134,9 +138,7 @@ def guidance_law(): accel[2] = accel[2]/(1-balance) #then we decrease it if the acceleration is over 2g - magnitude_accel=np.linalg.norm(accel) - if magnitude_accel>2*G: - accel=accel/magnitude_accel*2*G + accelerations=Vector3Stamped() accelerations.vector.x = accel[0] accelerations.vector.y=accel[1] @@ -211,7 +213,7 @@ def accel_publisher(): if __name__ == '__main__': G=9.8 - one_degree=np.pi/180 + one_degree=5*np.pi/180 pose = None velocity = None #describe constants for acceleration diff --git a/src/obstacle_test.py b/src/obstacle_test.py index 73dcdb9..b0aee79 100755 --- a/src/obstacle_test.py +++ b/src/obstacle_test.py @@ -27,13 +27,13 @@ def intersection(line1, line2): def distance_to_line(line1,vector1, center1, line2, bound1, bound2): intersect = intersection(line1,line2) #if the intersection point is outside where the line actually exists, return inifinity - if(np.all(intersect>bound1 and intercetbound1[0] and intersect[1]>bound1[1] and intersect[0]