82 lines
2.2 KiB
Rust
82 lines
2.2 KiB
Rust
|
use glam::{vec3, EulerRot};
|
||
|
|
use nalgebra::{self as na, Vector3};
|
||
|
|
use rapier3d::prelude as rp;
|
||
|
|
use std::{any::Any, f32};
|
||
|
|
|
||
|
|
use crate::drone::controller::DroneController;
|
||
|
|
pub use crate::drone::controller::JoystickInput;
|
||
|
|
use crate::drone::MotorCharacteristics;
|
||
|
|
|
||
|
|
#[derive(Default)]
|
||
|
|
pub struct PIDControllerState {
|
||
|
|
last_time: f32,
|
||
|
|
current_time: f32,
|
||
|
|
last_rotation: na::Unit<na::Quaternion<f32>>,
|
||
|
|
current_rotation: na::Unit<na::Quaternion<f32>>,
|
||
|
|
}
|
||
|
|
|
||
|
|
impl PIDControllerState {
|
||
|
|
pub fn dt(&self) -> f32 {
|
||
|
|
return self.last_time - self.current_time;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
pub struct PIDController {
|
||
|
|
input: JoystickInput,
|
||
|
|
pub target_rate: f32,
|
||
|
|
state: PIDControllerState,
|
||
|
|
}
|
||
|
|
|
||
|
|
impl Default for PIDController {
|
||
|
|
fn default() -> Self {
|
||
|
|
Self {
|
||
|
|
input: JoystickInput::default(),
|
||
|
|
target_rate: f32::consts::PI,
|
||
|
|
state: Default::default(),
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
impl PIDController {
|
||
|
|
pub fn set_input(&mut self, input: JoystickInput) {
|
||
|
|
self.input = input;
|
||
|
|
}
|
||
|
|
pub fn get_angular_velocity(&self) {
|
||
|
|
let dt: f32 = self.state.dt();
|
||
|
|
let angles =
|
||
|
|
(self.state.current_rotation.inverse() * self.state.last_rotation).euler_angles();
|
||
|
|
// roll,pitch.yaw
|
||
|
|
let vel_roll = angles.0 / dt;
|
||
|
|
let vel_pitch = angles.1 / dt;
|
||
|
|
let vel_yaw = angles.2 / dt;
|
||
|
|
println!("{},{},{}", vel_roll, vel_pitch, vel_yaw);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
impl DroneController for PIDController {
|
||
|
|
fn set_position(&mut self, position: rp::Isometry<f32>) {
|
||
|
|
self.state.last_rotation = self.state.current_rotation;
|
||
|
|
self.state.current_rotation = position.rotation;
|
||
|
|
}
|
||
|
|
fn set_time(&mut self, time: f32) {
|
||
|
|
self.state.last_time = self.state.current_time;
|
||
|
|
self.state.current_time = time;
|
||
|
|
}
|
||
|
|
fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
|
||
|
|
fn get_motor_throttles(&self) -> [f32; 4] {
|
||
|
|
self.get_angular_velocity();
|
||
|
|
return [1.0, 1.0, 1.0, 1.0];
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* These should be implemented like this for all the classes that implement DroneController
|
||
|
|
* Works as an example implementation
|
||
|
|
*/
|
||
|
|
fn as_any(&self) -> &dyn Any {
|
||
|
|
self
|
||
|
|
}
|
||
|
|
fn as_mut_any(&mut self) -> &mut dyn Any {
|
||
|
|
self
|
||
|
|
}
|
||
|
|
}
|