RustPhysicsMQ/src/drone/pidcontroller.rs

use nalgebra::{self as na, Vector3};
use rapier3d::prelude as rp;
use std::{any::Any, f32, io::PipeReader};

use crate::drone::controller::DroneController;
pub use crate::drone::controller::JoystickInput;
use crate::drone::MotorCharacteristics;

#[derive(Default)]
pub struct PControllerState {
    last_time: f32,
    current_time: f32,
    last_rotation: na::Unit<na::Quaternion<f32>>,
    current_rotation: na::Unit<na::Quaternion<f32>>,
    angular_velocity: na::Vector3<f32>,
}

impl PControllerState {
    pub fn dt(&self) -> f32 {
        return self.current_time - self.last_time;
    }
}

pub struct PController {
    input: JoystickInput,
    pub target_rate: f32,
    state: PControllerState,
}

impl Default for PController {
    fn default() -> Self {
        Self {
            input: JoystickInput::default(),
            target_rate: f32::consts::PI,
            state: Default::default(),
        }
    }
}

impl PController {
    pub fn set_input(&mut self, input: JoystickInput) {
        self.input = input;
    }
    pub fn get_angular_velocity(&self) -> na::Vector3<f32> {
        return self.state.angular_velocity;
    }

    pub fn get_desired_angular_velocity(&self) -> na::Vector3<f32> {
        let coords = na::Vector3::new(
            self.input.pitch_input,
            self.input.yaw_input,
            self.input.roll_input,
        ) * self.target_rate;
        return coords;
    }
}

impl DroneController for PController {
    fn set_rotation(&mut self, rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {
        self.state.last_rotation = self.state.current_rotation;
        self.state.current_rotation = rotation;
    }
    fn set_angular_velocity(&mut self, angvel: nalgebra::Vector3<f32>) {
        self.state.angular_velocity = angvel;
    }
    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(&mut self) -> [f32; 4] {
        let rot = self.state.current_rotation;
        let current = rot.inverse().transform_vector(&self.get_angular_velocity());
        let target = self.get_desired_angular_velocity();

        let diff = target - current;
        println!("Target: {:}", target);
        println!("Current: {:}", current);
        println!("Diff: {:}", diff);
        let throttle = self.input.throttle_input;

        let roll = diff.z * 0.01;

        let pitch = diff.x * 0.01;

        let yaw = diff.y;

        /*
         * Motor position indices
         *    ^ - Front
         *    |
         *    |
         * 1 --- 0
         * |     |
         * |     |
         * 2 --- 3
         */
        let mut motors = [
            throttle + yaw - pitch + roll,
            throttle - yaw - pitch - roll,
            throttle + yaw + pitch - roll,
            throttle - yaw + pitch + roll,
        ];

        // motors.max
        let max = motors
            .iter()
            .copied()
            .max_by(|a, b| a.total_cmp(b))
            .unwrap_or(0.0);
        if max > 1.0 {
            for v in motors.iter_mut() {
                *v /= max;
            }
        }
        return motors;
    }

    /*
     * 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
    }
}