winch_math.py

#!/usr/bin/env python3
import math

from scipy.integrate import quad 


# Starting distance (in meters)
start = 2.0

# Target distance (in meters)
target = 3.0

# Maximum speed (in m/s)
max_speed = 0.01

# Distance (in meters) from target at which we reduce to half speed. The smaller
# this value is, the more abruptly we stop.
half_speed_dist = 0.05

# Distance (in meters) at which we are close enough and stop
epsilon = 0.01


# Create a velocity function with respect to our position.
# 
# This is a sigmoid function ranging from - to +max_speed with zero at the
# target distance, reaching 1/2*max_speed at the half_speed_dist.
v = lambda x: max_speed * (2/math.pi) * math.atan((target - x) * (1 / half_speed_dist))


# Compute an estimate of the move duration using an integral approximation.
# Note we must estimate the time to our within-epsilon stopping distance, or
# else the integral will not converge.
t, _ = quad(
    lambda x: 1/v(x),
    start,
    target + (-1 if start < target else 1) * epsilon
)


# Set an upper bound on how long we will allow the motor to move as 110% of the
# estimated time, at least 10 seconds extra.
t_max = max(t * 1.10, t + 10.0)

# Set an envelope for our position
x_min = min(start, target) - 0.10
x_max = max(start, target) + 0.10


print(f'Moving from {start} m to {target} m')
print(f'Max velocity is {max_speed} m/s, decelerating to half within '
      f'{half_speed_dist} m of target')
print(f'Stopping within {epsilon} m of target')
print(f'It is expected to take {t//60:.0f} min {t%60:.0f} sec')
print(f'Emergency stop after {t_max//60:.0f} min {t_max%60:.0f} sec')
print(f'Emergency stop if position goes below {x_min} m or above {x_max} m')