Code: Select all
def do_deltas(deltas, output):
norm = [0, 0, 0]
dist = math.sqrt(
deltas[0] * deltas[0]
+ deltas[1] * deltas[1]
+ deltas[2] * deltas[2])
value = 40 * rot_gains_divided_by_40.getY(dist)
diagnostics.watch(value)
for k in range(3):
c = clamp(math.fabs(deltas[k]) / dist, 0, 1) if dist > 10**-6 else 0
norm[k] = c
diagnostics.watch(norm[0])
diagnostics.watch(norm[1])
diagnostics.watch(norm[2])
n = sum(norm)
diagnostics.watch(n)
if n > 10**-6:
ret = 1 / n
for k in range(3):
norm[k] *= ret
else:
for k in range(3):
norm[k] = 0
for k in range(3):
d = norm[k] * value
output[k] = math.copysign(d, deltas[k])
def accela_filter(input, output):
global accela_last_output
global accela_deltas
global accela_time_stamp
global ROTATION_THRESHOLD
global ROTATION_DEADZONE
dt = time.time() - accela_time_stamp
accela_time_stamp = time.time()
diagnostics.watch(dt)
for i in range(3):
d = input[i] - accela_last_output[i]
if math.fabs(d) > math.radians(ROTATION_DEADZONE):
d -= math.copysign(math.radians(ROTATION_DEADZONE), d)
else:
d = 0
diagnostics.watch(d)
accela_deltas[i] = d / math.radians(ROTATION_THRESHOLD)
diagnostics.watch(accela_deltas[0])
do_deltas(accela_deltas, output)
for k in range(3):
output[k] *= dt
output[k] += accela_last_output[k]
accela_last_output[k] = output[k]