increased acceleration

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:
+    if magnitude_velocity<3:
-        magnitude_forward_accel = 5-magnitude_velocity
+        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:
+    if magnitude_accel>0.5*G:
-        accel_straight=accel_straight/magnitude_accel*2*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)

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()