WIP #Hackathon - Force Feedback on Feetech STS3215

[Grochowicz]

What this does

This is a WIP by team 281 (L.A.E.L.E) for force feedback on STS3215 servo motor leader/follower, made during the LeRobot WorldWide Hackathon 2025.

Contributors: @LuizaMannes, @Askeiroh, @equadras, @leonardonm15, @Grochowicz.

How it works

The main logic is in teleoperate.py, which will now do two-way position information exchange between leader and follower.

There are threshold values that may need tweaking.

The code implements a traffic-light-like state machine and a virtual stall state machine:

Virtual stall:
The virtual stall state machine is used to make the motor current peak (because of a physical stall) set the virtual stall. The virtual stall is unset as soon as the position changes.

Traffic-light:
Green State – The follower is not struggling, so the operator can move the motor freely.
Yellow State – The follower is struggling.
Red State – The follower was blocked and the leader is in an invalid position. The leader is FORCED to imitate the follower’s position until things calm down. The operator loses control until it stabilizes.

Traffic-light State Transitions:
Green ➝ Red: if the motor just stalled.
Red ➝ Yellow: once the current drops back to safe levels (no more external force on the motor).
Yellow ➝ Red: if the motor just stalled.
Yellow ➝ Yellow: if the virtual stall is false.
Yellow ➝ Green: if the virtual stall is false.

TODO

• Make force feedback optional
• Make parameters more easily editable
• Print current state and stall status in teleoperate loop
• Clean up code
• Have calibration set current threshold