File content as of revision 80:38e274c4dafa:

#include "PIDController.h"
#include "LCDController.h"

static DigitalOut heater(p18);

static DRV8825 mtr_fresh(p21, p27, p28, p29, p22, p23);
static DRV8825 mtr_salt(p24, p27, p28, p29, p25, p26);


// Make sure heater is off
PIDController::~PIDController() {
    printf("PIDController destructor called, heater = 0\r\n");
    heater = 0;    
}

// This is called in the main loop on every iteration
void PIDController::update() {
    
    #ifndef TEST_MODE
    if(this->num_iters < STARTUP_ITERATIONS) {
        printf("PIDController: not running, startup phase");
        return;
    }
    #endif
    
    // Control the heater
    // This could be done in the pumping function as well, if needed
    if (this->isHeating() && this->temp->getValue() >= TEMP_MIN_UNDESIRED + 1.0f) {
        this->setHeating(false);
    } else if (!this->isHeating() && this->temp->getValue() < TEMP_MIN_UNDESIRED) {
        this->setHeating(true);
    }

    float s = this->salt->getValue();
    float s_in_grams = this->getSaltInGrams();
    
    if(s < MIN_VALID_SALINITY) {
        cout << "PIDController: Salinity NC, no stabilizing action";
    } else if(s <= SALT_MIN_UNDESIRED) {
        
        float ml = this->getMlSaltyWater(s_in_grams, this->proximity->getValue());
        int ml_int = std::min(static_cast<int>(ml), 30);
        ml_int = std::max(1, ml_int);
    
        cout << "PIDCONTROLLER: need to pump " << ml_int << " (" << ml << ")" << " ml of salty water\r\n";
        printf("need to pump %d ml of salty water\r\n",ml_int);
        
        this->pumpSaltWater(ml_int);
    
    } else if(s >= SALT_MAX_UNDESIRED) {

        float ml = this->getMlFreshWater(s, this->proximity->getValue());
        int ml_int = std::min(static_cast<int>(ml), 30);
        ml_int = std::max(1, ml_int);
        
        cout << "PIDCONTROLLER: need to pump " << ml_int << " (" << ml << ")" << " ml of fresh water\r\n";
        printf("need to pump %d ml of fresh water\r\n",ml_int);
        
        this->pumpFreshWater(ml_int);     
        
    }
}

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

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


void PIDController::pumpSaltWater(int ml) {
    this->pumpWater(&mtr_salt, ml);   
}


void PIDController::pumpFreshWater(int ml) {
    this->pumpWater(&mtr_fresh, ml);
}


void PIDController::pumpWater(DRV8825 *mtr, int ml) {
    LCDController::showPumping();
    
    this->pumping = true;
    
    int j = 5010 * (ml - 1);
    
    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(5);
    
    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(5);    
    this->pumping = false;
}


float PIDController::getMlSaltyWater(float grams_in_body, float volume_in_body) {
    
    float solution_gram_per_ten = 1.16f; // 1.0f for our solution
    float sol_volume = 10; // 10ml
    
    float ideal_ppt_constant = 0.007f; //7 ppt / 1000
    float x1 = volume_in_body * ideal_ppt_constant;
    float x2 = sol_volume * ideal_ppt_constant;
    
    x1 = x1 - grams_in_body;
    x2 = solution_gram_per_ten - x2;
    
    float output_ml = (x1 / x2 * sol_volume);
    
    return output_ml; // amount in ml to get 7 ppt.
}

bool PIDController::isHeating() {
    return this->heating;    
}

bool PIDController::isPumping() {
    return this->pumping;    
}

void PIDController::setHeating(bool enabled) {
    if(enabled == this->heating) return;
    
    this->heating = enabled;
    if(enabled) {
        #ifdef TEST_MODE
        printf("Should set heater to 1\r\n");
        #else
        heater = 1;
        #endif            
    } else {
        #ifdef TEST_MODE
        printf("Should set heater to 0\r\n");
        #else
        heater = 0;
        #endif    
    }
}

float PIDController::getMlFreshWater(float ppt, float volume) {
    return (volume * (ppt / 7.0)) - volume;
}

void PIDController::doTestingStuff(int ml) {
    while(1) {
        this->pumpSaltWater(ml);
        //this->pumpFreshWater(ml);
    }
}