File content as of revision 32:1e4919a44196:

#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 ml = this->getMlSaltyWater(sInGrams, this->proximity->getValue());
    
        printf("PIDCONTROLLER: need to pump %.3f ml of salty water\r\n", ml);
        
        // MAYBE DO SOME ROUNDING HERE
    
        // 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        
        
    }
}

std::string PIDController::get_name() {
    return "PIDController";
}

//get the salt in the body in grams
float PIDController::getSaltInGrams()
{
    float currentppt = this->salt->getValue(); //in ppt
    float currentvol = this->proximity->getValue(); //in ml
    
    return currentppt * (currentvol /1000);
}


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;
}