From 1cbbaeb283de5f2e09d787c4d19cf6d920d02ad2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 29 Jun 2023 19:31:44 +0900 Subject: [PATCH] Rover: circle uses lower accel and fix forward-back accel --- Rover/mode_circle.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp index 8368881274..42d6f63833 100644 --- a/Rover/mode_circle.cpp +++ b/Rover/mode_circle.cpp @@ -155,8 +155,9 @@ void ModeCircle::update() // accelerate speed up to desired speed const float speed_max = reached_edge ? config.speed : 0.0; - const float speed_change_max = (g2.pos_control.get_accel_max() * rover.G_Dt); - target.speed = constrain_float(speed_max, target.speed - speed_change_max, target.speed + speed_change_max); + const float speed_change_max = (g2.pos_control.get_accel_max() * 0.5 * rover.G_Dt); + const float accel_fb = constrain_float(speed_max - target.speed, -speed_change_max, speed_change_max); + target.speed += accel_fb; // calculate angular rate and update target angle const float circumference = 2.0 * M_PI * config.radius; @@ -174,8 +175,8 @@ void ModeCircle::update() target.vel.rotate(target.yaw_rad + radians(90)); // acceleration is towards center of circle and is speed^2 / radius - target.accel = Vector2f(sq(target.speed) / config.radius, 0); - target.accel.rotate(target.yaw_rad + radians(180)); + target.accel = Vector2f(-sq(target.speed) / config.radius, accel_fb / rover.G_Dt); + target.accel.rotate(target.yaw_rad); g2.pos_control.set_pos_vel_accel_target(target.pos, target.vel, target.accel); g2.pos_control.update(rover.G_Dt);