RustPhysicsMQ/src/drone/controller.rs

use rapier3d::prelude as rp;
use std::any::Any;

use crate::drone::MotorCharacteristics;

#[derive(Default)]
pub struct JoystickInput {
    // Value should be between 0 and 1
    pub throttle_input: f32,

    // Rotation Directions: https://upload.wikimedia.org/wikipedia/commons/c/c1/Yaw_Axis_Corrected.svg
    /*
     * Values should be between -1 and 1.
     */
    pub yaw_input: f32,
    pub pitch_input: f32,
    pub roll_input: f32,
}

impl JoystickInput {
    pub fn clamp(&self) -> JoystickInput {
        return JoystickInput {
            throttle_input: self.throttle_input.clamp(0.0, 1.0),
            yaw_input: self.yaw_input.clamp(-1.0, 1.0),
            pitch_input: self.pitch_input.clamp(-1.0, 1.0),
            roll_input: self.roll_input.clamp(-1.0, 1.0),
        };
    }
}

pub trait DroneController {
    // Allow downcast of trait -> class.
    //
    // This gives us the ability to have a Box<dyn DroneController>
    // And transform it into a pointer to its class.
    fn as_any(&self) -> &dyn Any;
    fn as_mut_any(&mut self) -> &mut dyn Any;

    /*
     * Methods called by Drone, to transmit information to the controller.
     */
    fn set_position(&mut self, position: rp::Isometry<f32>);
    fn set_time(&mut self, time: f32);
    fn set_motor_characteristics(&self, motor_characteristics: &MotorCharacteristics);

    /*
     * Throttle should be between 0 and 1. Values will be by the Drone class.
     */
    fn get_motor_throttles(&mut self) -> [f32; 4];
}

/*
* DefaultController just sets throttle to motor_throttle, a parameter passed to it on its contructor
*/
pub struct DefaultController {
    motor_throttle: f32,
}

impl DefaultController {
    pub fn new(motor_speed: f32) -> DefaultController {
        return DefaultController {
            motor_throttle: motor_speed,
        };
    }
}

impl DroneController for DefaultController {
    fn set_position(&mut self, _position: rp::Isometry<f32>) {}
    fn set_time(&mut self, _time: f32) {}
    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
    fn get_motor_throttles(&mut self) -> [f32; 4] {
        return [self.motor_throttle; 4];
    }

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