From ff1da749826cb0d5282817edf902f7c81da4e91d Mon Sep 17 00:00:00 2001
From: ahsueh1996 <ahsueh1996@gmail.com>
Date: Tue, 14 Jul 2015 12:54:46 -0400
Subject: [PATCH] update2() function in the simfpga is the simulator for yaw
 and depth data. Use as SUBZERO in sim mode as if using update().

@Jon, to get the damped oscillator instead of the overdamped... we think
we need float or double variables for all the fpga members.
---
 SubZero/src/settings/settings.txt |  3 +++
 SubZero/src/simulator/SimFPGA.cpp | 39 ++++++++++++++++++++++++-------
 SubZero/src/simulator/SimFPGA.h   | 10 ++++++--
 3 files changed, 42 insertions(+), 10 deletions(-)

diff --git a/SubZero/src/settings/settings.txt b/SubZero/src/settings/settings.txt
index 18ca072..1ddbb29 100644
--- a/SubZero/src/settings/settings.txt
+++ b/SubZero/src/settings/settings.txt
@@ -47,5 +47,8 @@ SIMSUB_START_X = 50.0
 SIMSUB_START_Y = 50.0
 SIMSUB_START_Z = 50.0
 SIMSUB_START_YAW = 30.0
+SIMSUB_START_SPEED = 2
+SIMSUB_START_DEPTH_SPEED = 1
+SIMSUB_START_ANGULAR_SPEED = 1
 
 SIMSUB_UPDATE_FREQ = 20.0
diff --git a/SubZero/src/simulator/SimFPGA.cpp b/SubZero/src/simulator/SimFPGA.cpp
index bdfdcf7..d1c1029 100644
--- a/SubZero/src/simulator/SimFPGA.cpp
+++ b/SubZero/src/simulator/SimFPGA.cpp
@@ -14,9 +14,9 @@ SimFPGA::SimFPGA(Properties* properties) {
     position.y = std::stod(properties->getProperty("SIMSUB_START_Y"));
     position.z = std::stod(properties->getProperty("SIMSUB_START_Z"));
     yaw = std::stod(properties->getProperty("SIMSUB_START_YAW"));
-    speed = 0;
-    depth_speed = 0;
-    angular_speed = 0;
+    speed = std::stoi(properties->getProperty("SIMSUB_START_SPEED"));;
+    depth_speed = std::stoi(properties->getProperty("SIMSUB_START_DEPTH_SPEED"));;
+    angular_speed = std::stoi(properties->getProperty("SIMSUB_START_ANGULAR_SPEED"));;
     accel = 0;
     depth_accel = 0;
     angular_accel = 0;
@@ -44,7 +44,8 @@ void SimFPGA::updateLoop() {
         if (timeElapsed > update_period) {
             timer.start();
             if(power && motors) {
-                update(timeElapsed);
+                //update(timeElapsed);
+                update2(timeElapsed);
             }
         }
     }
@@ -99,6 +100,32 @@ void SimFPGA::update(double period) {
     //TODO call code to update simulator engine's sub's position and yaw
 }
 
+void SimFPGA::update2(double timeElapsed) {
+    //assume all speed in per second units
+
+    //updating z
+    if (position.z > target_depth)
+        position.z -= depth_speed * timeElapsed;
+    else if (position.z < target_depth)
+        position.z += depth_speed * timeElapsed;
+    else
+        continue;
+
+    //updating x
+    position.x += speed;
+
+    //updating y
+    //we don't care :D atm
+
+    //updating yaw
+    target_yaw=target_yaw%360;
+    if (yaw < target_yaw)
+        yaw += angular_speed * timeElapsed;
+    else if (yaw > target_yaw)
+        yaw -= angular_speed * timeElapsed;
+    else
+        continue;
+}
 
 void SimFPGA::power_on() {
     power = true;
@@ -142,7 +169,3 @@ int SimFPGA::get_yaw() {
 int SimFPGA::get_depth() {
     return -(int)position.z;
 }
-
-
-
-
diff --git a/SubZero/src/simulator/SimFPGA.h b/SubZero/src/simulator/SimFPGA.h
index 9878602..14c432b 100644
--- a/SubZero/src/simulator/SimFPGA.h
+++ b/SubZero/src/simulator/SimFPGA.h
@@ -17,6 +17,12 @@ typedef struct {
     double z;
 } sim_position;
 
+enum dir {
+    X,
+    Y,
+    Z
+};
+
 #define ACCEL 5
 #define DEPTH_ACCEL 5
 #define ANGULAR_ACCEL 3
@@ -54,7 +60,7 @@ class SimFPGA {
 
     void updateLoop();
     void update(double period);
-
+    void update2(double period);
 
 public:
 
@@ -83,7 +89,7 @@ class SimFPGA {
     void power_off();
     int get_power();
 
-    void set_target_speed(int);
+    void set_target_speed(int speed);
     void set_target_depth(int);
     void set_target_yaw(int);