Diff: PIDController.cpp

Revision:

32:1e4919a44196

Parent:

30:cf12566013a5

Child:

35:c9261391a995

--- a/PIDController.cpp	Tue Jun 14 10:51:15 2016 +0000
+++ b/PIDController.cpp	Tue Jun 14 11:12:44 2016 +0000
@@ -1,22 +1,35 @@
 #include "PIDController.h"
 #include "SalinityController.h"
 
+DRV8825 mtr_fresh(p21, p27, p28, p29, p22, p23);
+DRV8825 mtr_salt(p24, p27, p28, p29, p25, p26);
+
+
 // This is called in the main loop on every iteration
 void PIDController::update() {
     // You can use the variables temp, salt and proximity like this:
     float s = this->salt->getValue();
+    float sInGrams = this->getSaltInGrams();
     
-    if(s < 6.0) {
-        float sInGrams = this->getSaltInGrams();
+    if(s <= 6.0) {
         
-        float ml = SalinityController::getMl(sInGrams, this->proximity->getValue());
+        float ml = this->getMlSaltyWater(sInGrams, this->proximity->getValue());
     
-        printf("PIDCONTROLLER: need to pump %.3f ml\r\n", ml);
+        printf("PIDCONTROLLER: need to pump %.3f ml of salty water\r\n", ml);
         
         // MAYBE DO SOME ROUNDING HERE
     
-        // DO SHIT WITH MOTORS AND STUFF
+        // CALL this->pump_salt_water
     
+    } else if(s >= 9.0) {
+
+        float ml = this->getMlFreshWater(s, this->proximity->getValue());
+        printf("PIDCONTROLLER: need to pump %.3f ml of fresh water\r\n", ml);
+        
+        // MAYBE DO SOME ROUNDING HERE
+    
+        // CALL this->pump_fresh_water        
+        
     }
 }
 
@@ -34,4 +47,66 @@
 }
 
 
-        
+void PIDController::pump_salt_water(int ml) {
+    this->pump_water(&mtr_salt, ml);   
+}
+
+
+void PIDController::pump_fresh_water(int ml) {
+    this->pump_water(&mtr_fresh, ml);
+}
+
+void PIDController::pump_water(DRV8825 *mtr, int ml) {
+    int j = 5010 * (ml - 1);
+    
+    for (int i = 500; i < MAX_SPEED; i += 5) {
+        mtr->settings(1 / MICROSTEPS_PER_STEP, LEFT, i);
+    }
+    
+    for (int i = 0; i < 2010 + j; i += 1) {
+        mtr->settings(1 / MICROSTEPS_PER_STEP, LEFT, 8000);
+    }
+    
+    for (int i = 8000; i > 500; i -= 5) {
+        mtr->settings(1 / MICROSTEPS_PER_STEP, LEFT, i);
+    }
+    
+    wait(3);
+    
+    for (int i = 500; i < MAX_SPEED; i += 5) {
+        mtr->settings(1 / MICROSTEPS_PER_STEP, RIGHT, i);
+    }
+    
+    for (int i = 0; i < 2010 + j; i += 1) {
+        mtr->settings(1 / MICROSTEPS_PER_STEP, RIGHT, 8000);
+    }
+    
+    for (int i = 8000; i > 500; i -= 5) {
+        mtr->settings(1 / MICROSTEPS_PER_STEP, RIGHT, i);
+    }
+    wait(3);
+    
+}
+
+
+float PIDController::getMlSaltyWater(float gramsinbody, float volumeinbody) {
+    
+    float solvolume = 10; // 10ml
+    
+    float idealpptconstant = 0.007f; //7 ppt / 1000
+    float x1 = volumeinbody * idealpptconstant;
+    float x2 = solvolume * idealpptconstant;
+    
+    x1 = x1 - gramsinbody;
+    x2 = 1 - x2;
+    
+    float outputml = (x1 / x2 * solvolume);
+    
+    return outputml; // amount in ml to get 7 ppt.
+    }
+
+
+float PIDController::getMlFreshWater(float ppt, float volume) {
+    return (volume * (ppt / 7.0)) - volume;
+}
+