File content as of revision 45:bb33913d4fd2:

#include "PIDController.h"
#include "testing.h"

DigitalOut heater(p18);

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() {
    
    #ifndef RUN_TESTS
    if(this->num_iters < STARTUP_ITERATIONS) {
        printf("PIDController: not running, startup phase\r\n");
        return;
    }
    #endif
    
    // Control the heater
    // This could be done in the pumping function as well, if needed
    this->set_heating(this->temp->getValue() < 32.0f);

    float s = this->salt->getValue();
    float sInGrams = this->getSaltInGrams();
    
    if(s <= 6.0) {
        
        float ml = this->getMlSaltyWater(sInGrams, this->proximity->getValue());
        int ml_int = static_cast<int>(ml);
    
        printf("PIDCONTROLLER: need to pump %d (%.3f) ml of salty water\r\n", ml_int, ml);
        
        //this->pump_salt_water(ml_int);
    
    } else if(s >= 9.0) {

        float ml = this->getMlFreshWater(s, this->proximity->getValue());
        int ml_int = static_cast<int>(ml);
        
        printf("PIDCONTROLLER: need to pump %d (%.3f) ml of fresh water\r\n", ml_int, ml);
        
        //this->pump_fresh_water(ml_int);     
        
    }
}

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

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

void PIDController::set_heating(bool enabled) {
    if(enabled == this->heating) return;
    
    this->heating = enabled;
    if(enabled) {
        #ifdef RUN_TESTS
        printf("Should set heater to 1\r\n");
        #else
        heater = 1;
        #endif            
    } else {
        #ifdef RUN_TESTS
        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;
}