Rover: circle mode loses input shaping

This commit is contained in:
Randy Mackay 2023-06-29 16:52:59 +09:00
parent 9d60aca85a
commit f1e3546e9e

View File

@ -177,7 +177,7 @@ void ModeCircle::update()
target.accel = Vector2f(sq(target.speed) / config.radius, 0);
target.accel.rotate(target.yaw_rad + radians(180));
g2.pos_control.input_pos_vel_accel_target(target.pos, target.vel, target.accel, rover.G_Dt);
g2.pos_control.set_pos_vel_accel_target(target.pos, target.vel, target.accel);
g2.pos_control.update(rover.G_Dt);
// get desired speed and turn rate from pos_control