Allow to control the motor in pos/vel with PD controller

[pierfabre]

closes #67

Todo

• Deal with the delay
• Implement velocity feedforward
  • measure the motor velocity we get for moto_command=1
• Tune gains

[pierfabre]

20250819 - Update

• Fixed delay issue
• Velocity feedforward
• Higher max vel (+/-30rad/s peak)
• Loaded

const float MOTOR_MAX_VEL = 150.0;  // rad/s
float Kff = 1.5 / MOTOR_MAX_VEL;
float Kp = 5.0;
float Kv = 0.02;
float TAU = 0.005;

state	action

VID-20250819-WA0000.mp4