changes to controller, had forgotten dt

configurations/PID_med+.toml

@@ -4,8 +4,8 @@ drone_tick_rate = 600
 target_rate = 3.141592
 
 proportional_multiplier = [0.10, 2.0, 0.10]
-integral_multiplier = [0.0000001, 0.00000001, 0.0000001]
+integral_multiplier = [0.001, 0.001, 0.001]
-diferential_multiplier = [0.0000001, 0.0000001, 0.0000001]
+diferential_multiplier = [0.0001, 0.0001, 0.0001]
 
 max_thrust = 2.6
 max_torque = 0.5

configurations/PI_med+.toml

@@ -4,7 +4,7 @@ drone_tick_rate = 600
 target_rate = 3.141592
 
 proportional_multiplier = [0.10, 2.0, 0.10]
-integral_multiplier = [0.0000001, 0.00000001, 0.0000001]
+integral_multiplier = [0.001, 0.0001, 0.001]
 diferential_multiplier = [0.0, 0.0, 0.0]
 
 max_thrust = 2.6

src/drone.rs

@@ -66,7 +66,7 @@ impl Drone {
     ) -> Drone {
         let drone_rb_handle = world.register_body(
             rp::RigidBodyBuilder::dynamic()
-                .translation(rp::vector![0.0, 100.0, 0.0])
+                .translation(rp::vector![0.0, 10.0, 0.0])
                 .rotation(rp::vector![0.0, 0.0, 0.0])
                 /*
                  * These damping values keep the simulation more realistic,

src/drone/pidcontroller.rs

@@ -123,7 +123,7 @@ impl DroneController for PIDController {
         let error: na::Vector3<f32> = target - current;
         let throttle = self.input.throttle_input;
 
-        let error_dif: na::Vector3<f32> = error - self.state.last_error;
+        let error_dif: na::Vector3<f32> = (error - self.state.last_error) / self.state.dt();
 
         let forces_to_apply = match PROPORTIONAL_ONLY {
             true => {
@@ -147,7 +147,7 @@ impl DroneController for PIDController {
                 ]
             }
         };
-        self.state.error_sum += error;
+        self.state.error_sum += error * self.state.dt();
         self.state.last_error = error;
 
         let pitch = forces_to_apply.x;

src/main.rs

@@ -1,6 +1,4 @@
-use gilrs;
 use macroquad::prelude as mq;
-use nalgebra as na;
 use rapier3d::prelude::*;
 
 mod engine;
@@ -30,7 +28,7 @@ fn window_conf() -> mq::Conf {
 #[macroquad::main(window_conf)]
 async fn main() {
     /* World Setup */
-    let mut world = World::default();
+    let mut world = World::new(600.0);
 
     world.register_free_collider(
         ColliderBuilder::cuboid(30.0, 1.0, 30.0)
@@ -42,9 +40,9 @@ async fn main() {
 
     let mut drone_controller = drone::pidcontroller::PIDController::default();
     drone_controller.set_input(JoystickInput {
-        throttle_input: 0.75,
+        throttle_input: 0.2,
-        yaw_input: 0.4,
+        yaw_input: 0.2,
-        roll_input: 0.7,
+        roll_input: 0.2,
         pitch_input: 0.0,
     });
     let mut drone_obj = drone::Drone::new(
@@ -72,8 +70,6 @@ async fn main() {
     );
     renderer.update_light(&world);
 
-    let mut input = JoystickInput::default();
-
     loop {
         renderer.update_camera(&world);
 
@@ -89,28 +85,10 @@ async fn main() {
         // Rendering
         renderer.draw(&mut world);
 
-        if world.tick % (20) == 0 {
+        if world.tick % (10) == 0 {
             // renderer.update_light(&world);
             // world.clear_ofb();
             mq::next_frame().await;
         }
     }
 }
-
-fn add_objects(world: &mut World) {
-    for i in 0..1 {
-        let body = world.register_body(
-            RigidBodyBuilder::dynamic()
-                .translation(vector![0.0, 50.0 + i as f32, 0.0])
-                .rotation(vector![std::f32::consts::PI / 4.2, i as f32, i as f32])
-                .build(),
-        );
-        world.register_collider(
-            ColliderBuilder::cuboid(2.0, 2.0, 2.0)
-                .restitution(0.1)
-                .build(),
-            body,
-            None,
-        );
-    }
-}

src/main_batch.rs

@@ -64,8 +64,8 @@ async fn main() {
 }
 
 async fn run_batch() {
-    fs::remove_dir_all(RESULTS_DIR).unwrap();
+    fs::remove_dir_all(RESULTS_DIR);
-    fs::create_dir_all(RESULTS_DIR).unwrap();
+    fs::create_dir_all(RESULTS_DIR);
 
     let input_files = list_files(INPUTS_DIR);
     let config_files = list_files(CONFIGS_DIR);