#!/bin/sh
#
# Copyright 2022 Stéphane Caron
# Copyright 2023 Inria
#
# See https://github.com/upkie/upkie_description#joint-limits

HIP_LIMIT_REV=0.2
KNEE_LIMIT_REV=0.25

echo "Configuring hip position limits..."
echo "conf set servopos.position_max ${HIP_LIMIT_REV}" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 1 -c
echo "conf set servopos.position_max ${HIP_LIMIT_REV}" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 4 -c
echo "conf set servopos.position_min -${HIP_LIMIT_REV}" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 1 -c
echo "conf set servopos.position_min -${HIP_LIMIT_REV}" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 4 -c

echo "Configuring hip velocity limits..."
echo "conf set servo.max_velocity 2.0" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 1 -c
echo "conf set servo.max_velocity 2.0" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 4 -c

echo "Configuring knee position limits..."
echo "conf set servopos.position_max ${KNEE_LIMIT_REV}" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 2 -c
echo "conf set servopos.position_max ${KNEE_LIMIT_REV}" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 5 -c
echo "conf set servopos.position_min -${KNEE_LIMIT_REV}" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 2 -c
echo "conf set servopos.position_min -${KNEE_LIMIT_REV}" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 5 -c

echo "Configuring knee velocity limits..."
echo "conf set servo.max_velocity 2.0" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 2 -c
echo "conf set servo.max_velocity 2.0" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 5 -c

echo "Configuring wheel position limits..."
echo "conf set servopos.position_max NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 3 -c
echo "conf set servopos.position_max NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 6 -c
echo "conf set servopos.position_min NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 3 -c
echo "conf set servopos.position_min NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 6 -c

echo "Configuring wheel velocity limits..."
echo "conf set servo.max_velocity 8.0" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 3 -c
echo "conf set servo.max_velocity 8.0" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 6 -c

# See https://github.com/mjbots/moteus/blob/38d688a933ce1584ee09f2628b5849d5e758ac21/docs/reference.md#servodefault_velocity_limit--servodefault_accel_limit
echo "Disabling interpolation velocity limits on all joints..."
echo "conf set servo.default_velocity_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 1 -c
echo "conf set servo.default_velocity_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 2 -c
echo "conf set servo.default_velocity_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 3 -c
echo "conf set servo.default_velocity_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 4 -c
echo "conf set servo.default_velocity_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 5 -c
echo "conf set servo.default_velocity_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 6 -c

# See https://github.com/mjbots/moteus/blob/38d688a933ce1584ee09f2628b5849d5e758ac21/docs/reference.md#servodefault_velocity_limit--servodefault_accel_limit
echo "Disabling acceleration limits on all joints..."
echo "conf set servo.default_accel_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 1 -c
echo "conf set servo.default_accel_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 2 -c
echo "conf set servo.default_accel_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 3 -c
echo "conf set servo.default_accel_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 4 -c
echo "conf set servo.default_accel_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 5 -c
echo "conf set servo.default_accel_limit NaN" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 6 -c

echo "Writing configuration to persistent storage..."
echo "conf write" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 1 -c
echo "conf write" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 2 -c
echo "conf write" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 3 -c
echo "conf write" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 4 -c
echo "conf write" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 5 -c
echo "conf write" | sudo moteus_tool --pi3hat-cfg "1=1,2,3;2=4,5,6" -t 6 -c
