From 382566a8da5653d84646bfc4d68894ab9f1bc37e Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Mon, 20 Sep 2021 11:35:11 -0700
Subject: [PATCH] changed up vehicle dynamics in accordance with simulink
 changes

---
 include/sim.h | 18 ++++++++++++------
 input.csv     |  4 ++--
 src/main.cpp  |  2 +-
 3 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/include/sim.h b/include/sim.h
index fc06bcb..93ed77b 100644
--- a/include/sim.h
+++ b/include/sim.h
@@ -313,12 +313,18 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
     State.rolldot = integral(State.rollddot, PrevState.rolldot, State.stepSize);
 
     // ax ay az
-    State.ax = (State.Fx / State.mass) +
-               (State.pitchdot * State.vz - State.rolldot * State.vy);
-    State.ay = (State.Fy / State.mass) +
-               (State.rolldot * State.vx - State.vz * State.yawdot);
-    State.az = (State.Fz / State.mass) +
-               (State.vy * State.yawdot - State.pitchdot * State.vx);
+    State.ax =
+        (State.Fx /
+         State.mass); // +
+                      //(State.pitchdot * State.vz - State.rolldot * State.vy);
+    State.ay =
+        (State.Fy /
+         State.mass); // +
+                      //(State.rolldot * State.vx - State.vz * State.yawdot);
+    State.az =
+        (State.Fz /
+         State.mass); // +
+                      //(State.vy * State.yawdot - State.pitchdot * State.vx);
 
     // vx vy vz in Body frame
     State.vx = integral(State.ax, PrevState.vx, State.stepSize);
diff --git a/input.csv b/input.csv
index c196115..2d989fe 100644
--- a/input.csv
+++ b/input.csv
@@ -16,5 +16,5 @@ Vehicle Radius,0.05,m
 Moment Arm,0.15,m
 Sim Step Size,1,ms
 Kp,-0.1,x
-Ki,0.1, x
-Kd,-0.5, x
+Ki,0, x
+Kd,0, x
diff --git a/src/main.cpp b/src/main.cpp
index c580c7e..d618714 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -64,7 +64,7 @@ int main() {
   // PID Gains
   State.Kp = varValueVec[17];
   State.Ki = varValueVec[18];
-  State.Kd = -1;
+  State.Kd = varValueVec[19];
 
   std::cout << State.Kd << "\n";