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

increased acceleration

This commit is contained in:
Matas Keras 2022-08-18 07:56:59 -04:00
parent b8ff019e66
commit 84cd4442a9
3 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
src/circle_
View File

18
src/obstacle_detection.py
View File

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

2
src/obstacle_test.py
View File

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