Skip to content

test: remove angular PID controller #21

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
tropicaaal wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

test: remove angular PID controller #21

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions examples/basic.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ use evian::prelude::*;
use vexide::prelude::*;

use evian::{
control::loops::{AngularPid, Pid},
control::loops::{Pid},
drivetrain::model::{Arcade, Differential},
motion::{Basic, Seeking},
tracking::wheeled::{TrackingWheel, WheeledTracking},
Expand All @@ -17,7 +17,7 @@ struct Robot {

impl Robot {
const LINEAR_PID: Pid = Pid::new(1.0, 0.0, 0.125, None);
const ANGULAR_PID: AngularPid = AngularPid::new(16.0, 0.0, 1.0, None);
const ANGULAR_PID: Pid = Pid::new(16.0, 0.0, 1.0, None);
const LINEAR_TOLERANCES: Tolerances = Tolerances::new()
.error(4.0)
.velocity(0.25)
Expand Down
2 changes: 1 addition & 1 deletion packages/evian-control/src/loops/mod.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ pub use feedforward::{
ArmFeedforward, ArmFeedforwardSetpoint, ElevatorFeedforward, ElevatorFeedforwardSetpoint,
MotorFeedforward, MotorFeedforwardSetpoint,
};
pub use pid::{AngularPid, Pid};
pub use pid::{Pid};
pub use tbh::TakeBackHalf;

/// Feedback ("closed-loop") controller.
Expand Down
151 changes: 0 additions & 151 deletions packages/evian-control/src/loops/pid.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -227,154 +227,3 @@ impl Feedback for Pid {
output
}
}

// MARK: Angular Controller

/// PID controller for use in rotational systems.
///
/// This struct operates on the same principles and implementation as [`Pid`], but takes exclusively
/// [`Angle`]s as input. Unlike [`Pid`], [`AngularPid`] is able to recognize when angles *wrap*.
/// This means a 0° measurement is equivalent to a 360° measurement, for instance.
///
/// This is useful for cases where you want the controller to drive the system to its setpoint using
/// the "shortest turn possible".
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct AngularPid {
kp: f64,
ki: f64,
kd: f64,

integral: f64,
output_limit: Option<f64>,
integration_range: Option<Angle>,
prev_error: Angle,
}

impl AngularPid {
/// Construct a new PID controller from gain constants and an optional integration range.
#[must_use]
pub const fn new(kp: f64, ki: f64, kd: f64, integration_range: Option<Angle>) -> Self {
Self {
kp,
ki,
kd,
integration_range,
integral: 0.0,
output_limit: None,
prev_error: Angle::from_radians(0.0),
}
}

/// Get the current PID gains as a tuple (`kp`, `ki`, `kd`).
#[must_use]
pub const fn gains(&self) -> (f64, f64, f64) {
(self.kp, self.ki, self.kd)
}

/// Returns the controller's proportional gain (`kp`).
#[must_use]
pub const fn kp(&self) -> f64 {
self.kp
}

/// Returns the controller's integral gain (`kp`).
#[must_use]
pub const fn ki(&self) -> f64 {
self.ki
}

/// Returns the controller's derivative gain (`kp`).
#[must_use]
pub const fn kd(&self) -> f64 {
self.kd
}

/// Returns the controller's integration range.
///
/// Integration range is the minimum error range required to start integrating error. This is
/// optionally applied to the controller as a mitigation for [integral windup].
///
/// [integral windup]: https://en.wikipedia.org/wiki/Integral_windup
#[must_use]
pub const fn integration_range(&self) -> Option<Angle> {
self.integration_range
}

/// Sets the PID gains to provided values.
pub const fn set_gains(&mut self, kp: f64, ki: f64, kd: f64) {
self.kp = kp;
self.ki = ki;
self.kd = kd;
}

/// Sets the controller's proportional gain (`kp`).
pub const fn set_kp(&mut self, kp: f64) {
self.kp = kp;
}

/// Sets the controller's integral gain (`ki`).
pub const fn set_ki(&mut self, ki: f64) {
self.ki = ki;
}

/// Sets the controller's derivative gain (`kd`).
pub const fn set_kd(&mut self, kd: f64) {
self.kd = kd;
}

/// Sets the controller's integration range.
///
/// Integration range is the minimum error range required to start integrating error. This is
/// optionally applied to the controller as a mitigation for [integral windup].
///
/// [integral windup]: https://en.wikipedia.org/wiki/Integral_windup
pub const fn set_integration_range(&mut self, range: Option<Angle>) {
self.integration_range = range;
}

/// Sets the controller's output limit.
///
/// This sets a maximum range for the controller's output signal. It will effectively limit how
/// fast the controller is able to drive the system, which may be desirable in some cases (e.g.
/// limiting the maximum speed of a robot's motion).
pub const fn set_output_limit(&mut self, range: Option<f64>) {
self.output_limit = range;
}
}

// MARK: Loop

impl Feedback for AngularPid {
type State = Angle;
type Signal = f64;

fn update(&mut self, measurement: Angle, setpoint: Angle, dt: Duration) -> f64 {
let error = (setpoint - measurement).wrapped_half();

// If an integration range is used and we are within it, add to the integral.
// If we are outside of the range, or if we have crossed the setpoint, reset integration.
#[allow(clippy::float_cmp)]
if self
.integration_range
.is_none_or(|range| error.as_radians().abs() < range.as_radians())
&& error.signum() == self.prev_error.signum()
{
self.integral += error.as_radians() * dt.as_secs_f64();
} else {
self.integral = 0.0;
}

// Calculate derivative (change in error / change in time)
let derivative = (error - self.prev_error).as_radians() / dt.as_secs_f64();
self.prev_error = error;

let mut output =
(error.as_radians() * self.kp) + (self.integral * self.ki) + (derivative * self.kd);

if let Some(range) = self.output_limit {
output = output.clamp(-range, range);
}

output
}
}
28 changes: 14 additions & 14 deletions packages/evian-motion/src/basic/drive.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ use vexide::time::{Sleep, sleep};

use evian_control::{
Tolerances,
loops::{AngularPid, Feedback, Pid},
loops::{Feedback, Pid},
};
use evian_drivetrain::{Drivetrain, model::Arcade};
use evian_math::Angle;
Expand All @@ -30,7 +30,7 @@ pub struct DriveFuture<'a, M, L, A, T>
where
M: Arcade,
L: Feedback<State = f64, Signal = f64> + Unpin,
A: Feedback<State = Angle, Signal = f64> + Unpin,
A: Feedback<State = f64, Signal = f64> + Unpin,
T: TracksForwardTravel + TracksHeading + TracksVelocity,
{
pub(crate) target_distance: f64,
Expand All @@ -52,7 +52,7 @@ impl<M, L, A, T> Future for DriveFuture<'_, M, L, A, T>
where
M: Arcade,
L: Feedback<State = f64, Signal = f64> + Unpin,
A: Feedback<State = Angle, Signal = f64> + Unpin,
A: Feedback<State = f64, Signal = f64> + Unpin,
T: TracksForwardTravel + TracksHeading + TracksVelocity,
{
type Output = ();
Expand All @@ -74,16 +74,14 @@ where
if Pin::new(&mut state.sleep).poll(cx).is_pending() {
return Poll::Pending;
}

let dt = state.prev_time.elapsed();

let forward_travel = this.drivetrain.tracking.forward_travel();
let heading = this.drivetrain.tracking.heading();

let linear_error = (this.target_distance + state.initial_forward_travel) - forward_travel;
let angular_error = (this.target_heading - heading).wrapped_half();

// println!("{}", linear_error);

if this
.linear_tolerances
Expand Down Expand Up @@ -112,9 +110,11 @@ where
this.target_distance + state.initial_forward_travel,
dt,
);
let angular_output = this
.angular_controller
.update(heading, this.target_heading, dt);
let angular_output = this.angular_controller.update(
heading.as_radians(),
this.target_heading.as_radians(),
dt,
);

drop(
this.drivetrain
Expand All @@ -136,7 +136,7 @@ impl<M, L, A, T> DriveFuture<'_, M, L, A, T>
where
M: Arcade,
L: Feedback<State = f64, Signal = f64> + Unpin,
A: Feedback<State = Angle, Signal = f64> + Unpin,
A: Feedback<State = f64, Signal = f64> + Unpin,
T: TracksForwardTravel + TracksHeading + TracksVelocity,
{
/// Modifies this motion's linear feedback controller.
Expand Down Expand Up @@ -260,7 +260,7 @@ where
impl<M, A, T> DriveFuture<'_, M, Pid, A, T>
where
M: Arcade,
A: Feedback<State = Angle, Signal = f64> + Unpin,
A: Feedback<State = f64, Signal = f64> + Unpin,
T: TracksForwardTravel + TracksHeading + TracksVelocity,
{
/// Modifies this motion's linear PID gains.
Expand Down Expand Up @@ -315,7 +315,7 @@ where

// MARK: Angular PID Modifiers

impl<M, L, T> DriveFuture<'_, M, L, AngularPid, T>
impl<M, L, T> DriveFuture<'_, M, L, Pid, T>
where
M: Arcade,
L: Feedback<State = f64, Signal = f64> + Unpin,
Expand Down Expand Up @@ -348,7 +348,7 @@ where
/// Modifies this motion's angular integration range.
pub const fn with_angular_integration_range(&mut self, integration_range: Angle) -> &mut Self {
self.angular_controller
.set_integration_range(Some(integration_range));
.set_integration_range(Some(integration_range.as_radians()));
self
}

Expand Down
4 changes: 2 additions & 2 deletions packages/evian-motion/src/basic/mod.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ pub use turn_to_point::TurnToPointFuture;
pub struct Basic<L, A>
where
L: Feedback<State = f64, Signal = f64> + Unpin + Clone,
A: Feedback<State = Angle, Signal = f64> + Unpin + Clone,
A: Feedback<State = f64, Signal = f64> + Unpin + Clone,
{
/// Linear (forward driving) feedback controller.
pub linear_controller: L,
Expand All @@ -39,7 +39,7 @@ where
impl<L, A> Basic<L, A>
where
L: Feedback<State = f64, Signal = f64> + Unpin + Clone,
A: Feedback<State = Angle, Signal = f64> + Unpin + Clone,
A: Feedback<State = f64, Signal = f64> + Unpin + Clone,
{
/// Moves the robot forwards by a given distance (measured in wheel units) while
/// turning to face a heading.
Expand Down
22 changes: 12 additions & 10 deletions packages/evian-motion/src/basic/turn_to_point.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ use vexide::time::{Sleep, sleep};

use evian_control::{
Tolerances,
loops::{AngularPid, Feedback, Pid},
loops::{Feedback, Pid},
};
use evian_drivetrain::{Drivetrain, model::Arcade};
use evian_math::{Angle, IntoAngle, Vec2};
Expand All @@ -30,7 +30,7 @@ pub struct TurnToPointFuture<'a, M, L, A, T>
where
M: Arcade,
L: Feedback<State = f64, Signal = f64> + Unpin,
A: Feedback<State = Angle, Signal = f64> + Unpin,
A: Feedback<State = f64, Signal = f64> + Unpin,
T: TracksPosition + TracksHeading + TracksVelocity,
{
pub(crate) point: Vec2<f64>,
Expand All @@ -51,7 +51,7 @@ impl<M, L, A, T> Future for TurnToPointFuture<'_, M, L, A, T>
where
M: Arcade,
L: Feedback<State = f64, Signal = f64> + Unpin,
A: Feedback<State = Angle, Signal = f64> + Unpin,
A: Feedback<State = f64, Signal = f64> + Unpin,
T: TracksForwardTravel + TracksHeading + TracksVelocity + TracksPosition,
{
type Output = ();
Expand Down Expand Up @@ -109,9 +109,11 @@ where
let linear_output =
this.linear_controller
.update(forward_travel, state.initial_forward_travel, dt);
let angular_output = this
.angular_controller
.update(-angular_error, Angle::ZERO, dt);
let angular_output = this.angular_controller.update(
-angular_error.as_radians(),
Angle::ZERO.as_radians(),
dt,
);

drop(
this.drivetrain
Expand All @@ -133,7 +135,7 @@ impl<M, L, A, T> TurnToPointFuture<'_, M, L, A, T>
where
M: Arcade,
L: Feedback<State = f64, Signal = f64> + Unpin,
A: Feedback<State = Angle, Signal = f64> + Unpin,
A: Feedback<State = f64, Signal = f64> + Unpin,
T: TracksPosition + TracksForwardTravel + TracksHeading + TracksVelocity,
{
/// Modifies this motion's linear feedback controller.
Expand Down Expand Up @@ -257,7 +259,7 @@ where
impl<M, A, T> TurnToPointFuture<'_, M, Pid, A, T>
where
M: Arcade,
A: Feedback<State = Angle, Signal = f64> + Unpin,
A: Feedback<State = f64, Signal = f64> + Unpin,
T: TracksPosition + TracksForwardTravel + TracksHeading + TracksVelocity,
{
/// Modifies this motion's linear PID gains.
Expand Down Expand Up @@ -312,7 +314,7 @@ where

// MARK: Angular PID Modifiers

impl<M, L, T> TurnToPointFuture<'_, M, L, AngularPid, T>
impl<M, L, T> TurnToPointFuture<'_, M, L, Pid, T>
where
M: Arcade,
L: Feedback<State = f64, Signal = f64> + Unpin,
Expand Down Expand Up @@ -345,7 +347,7 @@ where
/// Modifies this motion's angular integration range.
pub const fn with_angular_integration_range(&mut self, integration_range: Angle) -> &mut Self {
self.angular_controller
.set_integration_range(Some(integration_range));
.set_integration_range(Some(integration_range.as_radians()));
self
}

Expand Down
Loading
Loading