#!/bin/bash
#
# Copyright 2022 Stéphane Caron

PI3HAT_CFG="1=1,2,3;3=4,5,6"

for i in {1..6}; do
    echo "Setting zero offset on servo ${i}..."
    (set -x; sudo moteus_tool --pi3hat-cfg "${PI3HAT_CFG}" --zero-offset -t "$i")
done

UPKIE_TOOL=$(which upkie_tool)
if [[ -n "${UPKIE_TOOL}" ]]; then
    (set -x; sudo "${UPKIE_TOOL}" stats)
else
    echo "Warning: upkie_tool not found"
fi

touch /tmp/rezero_success
