From 01875c62dc3f6f4650b18db227ea4bedab1770fa Mon Sep 17 00:00:00 2001
From: Patrick McDonagh <pmcdonagh@henry-pump.com>
Date: Mon, 13 Mar 2017 15:24:07 -0500
Subject: [PATCH] Adds simulation for in-between measurements

---
 Well-Simulator.ino | 60 ++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 53 insertions(+), 7 deletions(-)

diff --git a/Well-Simulator.ino b/Well-Simulator.ino
index 3d312d4..c39bba4 100644
--- a/Well-Simulator.ino
+++ b/Well-Simulator.ino
@@ -6,6 +6,7 @@ Adafruit_MCP4725 dacLoad;
 
 // INPUT PARAMETERS
 long dt_ms = 30;
+long r = 10; // number of points to write between the given measurements
 const int num_points = 673;
 const double surface_position[num_points] = {
   0,        0.066215, 0.171112, 0.316819, 0.498586, 0.715024, 0.97621,  1.2883,
@@ -196,8 +197,14 @@ void setup() {
 
 }
 
-uint16_t p;
-uint16_t l;
+uint16_t p, p_next;
+uint16_t l, l_next;
+uint16_t p_vout, l_vout;
+
+double m_p, m_l, b_p, b_l;
+
+
+long t_n = dt_ms / r;
 
 uint16_t scale(double raw_inp, double raw_min, double raw_max, double scaled_min, double scaled_max){
   double scaled_float = ((scaled_max - scaled_min)/(raw_max - raw_min)) * raw_inp + (scaled_max - ((scaled_max - scaled_min)/(raw_max - raw_min)) * raw_max);
@@ -206,13 +213,52 @@ uint16_t scale(double raw_inp, double raw_min, double raw_max, double scaled_min
 
 void loop() {
   // put your main code here, to run repeatedly:
-  for(int i = 0; i < num_points; i++){
+  for(int i = 0; i < num_points - 1; i++){
     p = scale(surface_position[i], 0.0, 145.0, 0, 4096);
+    p_next = scale(surface_position[i+1], 0.0, 145.0, 0, 4096);
     l = scale(surface_load[i], 0.0, 50000.0, 0, 4096);
-    dacPosition.setVoltage(p, false);
-    dacLoad.setVoltage(l, false);
-    Serial.println("p: " + String(p) + ", l: " + String(l));
-    delay(dt_ms);
+    l_next = scale(surface_load[i+1], 0.0, 50000.0, 0, 4096);
+
+    m_p = (double(p_next) - double(p)) / double(dt_ms);
+    b_p = double(p);
+
+    m_l = (double(l_next) - double(l)) / double(dt_ms);
+    b_l = double(l);
+    
+    for(int n = 0; n < r; n++){
+      p_vout = m_p * n * t_n + b_p;
+      l_vout = m_l * n * t_n + b_l;
+      
+      dacPosition.setVoltage(p_vout, false);
+      dacLoad.setVoltage(l_vout, false);
+      
+      Serial.println(String(p_vout) + "," + String(l_vout));
+
+      delay(t_n);
+    }
   }
 
+  // Last point going to first point
+  p = scale(surface_position[num_points - 1], 0.0, 145.0, 0, 4096);
+  p_next = scale(surface_position[0], 0.0, 145.0, 0, 4096);
+  l = scale(surface_load[num_points - 1], 0.0, 50000.0, 0, 4096);
+  l_next = scale(surface_load[0], 0.0, 50000.0, 0, 4096);
+
+  m_p = (double(p_next) - double(p)) / double(dt_ms);
+  b_p = double(p);
+
+  m_l = (double(l_next) - double(l)) / double(dt_ms);
+  b_l = double(l);
+  
+  for(int n = 0; n < r; n++){
+    p_vout = m_p * n * t_n + b_p;
+    l_vout = m_l * n * t_n + b_l;
+    
+    dacPosition.setVoltage(p_vout, false);
+    dacLoad.setVoltage(l_vout, false);
+    
+    Serial.println(String(p_vout) + "," + String(l_vout));
+
+    delay(t_n);
+  }
 }