From ab2a0bc788fd10d55b1d8fb5ff9c2f72eb610d42 Mon Sep 17 00:00:00 2001
From: Franchioping <franchioping@gmail.com>
Date: Tue, 9 Dec 2025 19:23:57 +0000
Subject: [PATCH] unfinished PID controller. TODO-LATER add an air drag model

---
 src/drone.rs            |  7 ++++---
 src/drone/controller.rs | 33 +++++++++++++++++++++++++++++----
 src/main.rs             | 26 +++++++++++++++++---------
 3 files changed, 50 insertions(+), 16 deletions(-)

diff --git a/src/drone.rs b/src/drone.rs
index 5430c73..191d4dc 100644
--- a/src/drone.rs
+++ b/src/drone.rs
@@ -5,6 +5,7 @@ use crate::engine::{ColliderExtraData, World};
 
 pub mod controller;
 pub mod fpvcontroller;
+pub mod pidcontroller;
 use controller::*;
 
 pub struct MotorCharacteristics {
@@ -62,8 +63,8 @@ impl Drone {
                  * Values are kind of random for now. Calculating them requires the final model
                  * A Poor Man's fluid simulation :D
                  */
-                .linear_damping(0.1) // Damps velocity slowly
-                .angular_damping(0.1) // Damps angular velocity slowly
+                .linear_damping(0.2) // Damps velocity slowly
+                .angular_damping(0.2) // Damps angular velocity slowly
                 .build(),
         );
         world.register_collider(
@@ -136,8 +137,8 @@ impl Drone {
 
     fn update_controller(&mut self, world: &World) {
         let rb = world.bodies.get(self.rb_handle).unwrap();
-        self.controller.set_position(*rb.position());
         self.controller.set_time(world.get_time());
+        self.controller.set_position(*rb.position());
     }
 
     pub fn process_tick(&mut self, world: &mut World) {
diff --git a/src/drone/controller.rs b/src/drone/controller.rs
index a251ba0..7a7bfda 100644
--- a/src/drone/controller.rs
+++ b/src/drone/controller.rs
@@ -3,6 +3,31 @@ 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.
     //
@@ -14,8 +39,8 @@ pub trait DroneController {
     /*
      * Methods called by Drone, to transmit information to the controller.
      */
-    fn set_position(&self, position: rp::Isometry<f32>);
-    fn set_time(&self, time: f32);
+    fn set_position(&mut self, position: rp::Isometry<f32>);
+    fn set_time(&mut self, time: f32);
     fn set_motor_characteristics(&self, motor_characteristics: &MotorCharacteristics);
 
     /*
@@ -40,8 +65,8 @@ impl DefaultController {
 }
 
 impl DroneController for DefaultController {
-    fn set_position(&self, _position: rp::Isometry<f32>) {}
-    fn set_time(&self, _time: f32) {}
+    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(&self) -> [f32; 4] {
         return [self.motor_throttle; 4];
diff --git a/src/main.rs b/src/main.rs
index c6e5581..d9f1d53 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -44,18 +44,18 @@ async fn main() {
         None,
     );
 
-    let mut controller = drone::fpvcontroller::FPVController::default();
+    let mut controller = drone::pidcontroller::PIDController::default();
     controller.set_input(JoystickInput {
-        throttle_input: 0.25,
-        yaw_input: 0.0,
+        throttle_input: 1.0,
+        yaw_input: 0.05,
         roll_input: 0.0,
-        pitch_input: 0.05,
+        pitch_input: 0.0,
     });
     let mut drone = drone::Drone::new(
         &mut world,
         Box::new(controller),
         drone::MotorCharacteristics {
-            max_thrust: 20.0,
+            max_thrust: 3.0,
             max_torque: 2.0,
             ..Default::default()
         },
@@ -87,9 +87,18 @@ async fn main() {
         let _ = clearscreen::clear();
         drone.process_tick(&mut world);
         println!(
-            "{:}",
+            "Translation: {:}",
             world.bodies.get(drone.rb_handle).unwrap().translation()
         );
+        println!(
+            "Angular Velocity: {:}",
+            world.bodies.get(drone.rb_handle).unwrap().angvel()
+        );
+        println!(
+            "Velocity: {:}",
+            world.bodies.get(drone.rb_handle).unwrap().linvel()
+        );
+
         println!(
             "Motor Throttles: {:?}",
             drone.controller.get_motor_throttles()
@@ -98,12 +107,11 @@ async fn main() {
         // Rendering
         renderer.draw(&mut world);
 
-        if world.tick % 30 == 0 {
+        if world.tick % 60 == 0 {
             world.clear_ofb();
             renderer.update_light(&world);
+            mq::next_frame().await;
         }
-
-        mq::next_frame().await;
     }
 }