diff --git a/src/circle_ b/src/circle_ deleted file mode 100644 index e69de29..0000000 diff --git a/src/obstacle_detection.py b/src/obstacle_detection.py index aff78ca..92164b5 100755 --- a/src/obstacle_detection.py +++ b/src/obstacle_detection.py @@ -113,11 +113,11 @@ def avoidance(weights): #next we calculate the magnitude of the velocity in the xy direction, #and the magnitude of the acceleration vector needed magnitude_velocity = np.sqrt(velocity[1]*velocity[1]+velocity[0]*velocity[0]) - magnitude_acceleration = 0.1*(magnitude_velocity*magnitude_velocity)/(min_radius) + magnitude_acceleration = 1.2*(magnitude_velocity*magnitude_velocity)/(min_radius) #if the velocity is too low (below 5m/s) we also have an additional forward #acceleration - if magnitude_velocity<5: - magnitude_forward_accel = 5-magnitude_velocity + if magnitude_velocity<3: + magnitude_forward_accel = 3-magnitude_velocity else: magnitude_forward_accel = 0 #finally we calculate the acceleration in the x and the y directions @@ -151,11 +151,11 @@ def get_error_acceleration(circle_position, r, velocity): circle_unit_3 = np.dot(circle_position, unit_vector_3) accel_2_1 = -1*(b3*velocity_unit_2+k*circle_unit_2) - accel_2_2 = 2*(4.5-abs(velocity_unit_2))*velocity_unit_2/abs(velocity_unit_2) + accel_2_2 = 2*(3-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*(4.5-abs(velocity_unit_3))*velocity_unit_3/abs(velocity_unit_3) + accel_3_2 = 2*(3-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 @@ -165,7 +165,7 @@ def get_forward_acceleration(y, velocity): #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*(4.5-abs(forward_velocity))*forward_velocity/abs(forward_velocity) + forward_acceleration = 2*(3-abs(forward_velocity))*forward_velocity/abs(forward_velocity) 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 @@ -178,8 +178,8 @@ def guidance_law(balance_queue_input, balance_input): #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 + if magnitude_accel>0.5*G: + accel_straight=accel_straight/magnitude_accel*0.5*G #next we deal with the avoidance component avoidance_accel,min_radius = avoidance(weights) @@ -193,7 +193,7 @@ def guidance_law(balance_queue_input, balance_input): accelerations=Vector3Stamped() accelerations.vector.x = accel[0] accelerations.vector.y=accel[1] - accelerations.vector.z=accel[2] + accelerations.vector.z=accel[2]+1 #we also publish the minimum radius, and the balance for debugging purposes balance_pub.publish(balance) min_radius_pub.publish(min_radius) diff --git a/src/obstacle_test.py b/src/obstacle_test.py index c5747aa..972a401 100755 --- a/src/obstacle_test.py +++ b/src/obstacle_test.py @@ -121,7 +121,7 @@ if __name__ == '__main__': #([m,b],lower bound for x, upper bound for x) #where the line is defined by y=mx+b, but only exists when x is between #the lower and upper bounds. - wall_lines = [ ([-1.5,100],-500,500)] + wall_lines = [ ([-1,150],-100,100),([1,350],-100,100)] try: distance_publisher()