PIDController.cpp

00001 #include "PIDController.h"
00002 #include "LCDController.h"
00003 
00004 static DigitalOut heater(p18);
00005 
00006 static DRV8825 mtr_fresh(p21, p27, p28, p29, p22, p23);
00007 static DRV8825 mtr_salt(p24, p27, p28, p29, p25, p26);
00008 
00009 
00010 // Make sure heater is off
00011 PIDController::~PIDController() {
00012     printf("PIDController destructor called, heater = 0\r\n");
00013     heater = 0;    
00014 }
00015 
00016 // This is called in the main loop on every iteration
00017 void PIDController::update() {
00018     
00019     #ifndef TEST_MODE
00020     if(this->num_iters < STARTUP_ITERATIONS) {
00021         printf("PIDController: not running, startup phase");
00022         return;
00023     }
00024     #endif
00025     
00026     // Control the heater
00027     // This could be done in the pumping function as well, if needed
00028     if (this->isHeating() && this->temp->getValue() >= TEMP_MIN_UNDESIRED + 1.0f) {
00029         this->setHeating(false);
00030     } else if (!this->isHeating() && this->temp->getValue() < TEMP_MIN_UNDESIRED) {
00031         this->setHeating(true);
00032     }
00033 
00034     float s = this->salt->getValue();
00035     float s_in_grams = this->getSaltInGrams();
00036     
00037     if(s < MIN_VALID_SALINITY) {
00038         cout << "PIDController: Salinity NC, no stabilizing action";
00039     } else if(s <= SALT_MIN_UNDESIRED) {
00040         
00041         float ml = this->getMlSaltyWater(s_in_grams, this->proximity->getValue());
00042         int ml_int = std::min(static_cast<int>(ml), 30);
00043         ml_int = std::max(1, ml_int);
00044     
00045         cout << "PIDCONTROLLER: need to pump " << ml_int << " (" << ml << ")" << " ml of salty water\r\n";
00046         printf("need to pump %d ml of salty water\r\n",ml_int);
00047         
00048         this->pumpSaltWater(ml_int);
00049     
00050     } else if(s >= SALT_MAX_UNDESIRED) {
00051 
00052         float ml = this->getMlFreshWater(s, this->proximity->getValue());
00053         int ml_int = std::min(static_cast<int>(ml), 30);
00054         ml_int = std::max(1, ml_int);
00055         
00056         cout << "PIDCONTROLLER: need to pump " << ml_int << " (" << ml << ")" << " ml of fresh water\r\n";
00057         printf("need to pump %d ml of fresh water\r\n",ml_int);
00058         
00059         this->pumpFreshWater(ml_int);     
00060         
00061     }
00062 }
00063 
00064 std::string PIDController::getName() {
00065     return "PIDController";
00066 }
00067 
00068 //get the salt in the body in grams
00069 float PIDController::getSaltInGrams() {
00070     float current_ppt = this->salt->getValue(); //in ppt
00071     float current_vol = this->proximity->getValue(); //in ml
00072     
00073     return current_ppt * (current_vol / 1000);
00074 }
00075 
00076 
00077 void PIDController::pumpSaltWater(int ml) {
00078     this->pumpWater(&mtr_salt, ml);   
00079 }
00080 
00081 
00082 void PIDController::pumpFreshWater(int ml) {
00083     this->pumpWater(&mtr_fresh, ml);
00084 }
00085 
00086 
00087 void PIDController::pumpWater(DRV8825 *mtr, int ml) {
00088     LCDController::showPumping();
00089     
00090     this->pumping = true;
00091     
00092     int j = 5010 * (ml - 1);
00093     
00094     for (int i = 500; i < MAX_SPEED; i += 5) {
00095         mtr->settings(1 / MICROSTEPS_PER_STEP, RIGHT, i);
00096     }
00097     
00098     for (int i = 0; i < 2010 + j; i += 1) {
00099         mtr->settings(1 / MICROSTEPS_PER_STEP, RIGHT, 8000);
00100     }
00101     
00102     for (int i = 8000; i > 500; i -= 5) {
00103         mtr->settings(1 / MICROSTEPS_PER_STEP, RIGHT, i);
00104     }
00105     
00106     wait(5);
00107     
00108     for (int i = 500; i < MAX_SPEED; i += 5) {
00109         mtr->settings(1 / MICROSTEPS_PER_STEP, LEFT, i);
00110     }
00111     
00112     for (int i = 0; i < 2010 + j; i += 1) {
00113         mtr->settings(1 / MICROSTEPS_PER_STEP, LEFT, 8000);
00114     }
00115     
00116     for (int i = 8000; i > 500; i -= 5) {
00117         mtr->settings(1 / MICROSTEPS_PER_STEP, LEFT, i);
00118     }
00119     
00120     wait(5);    
00121     this->pumping = false;
00122 }
00123 
00124 
00125 float PIDController::getMlSaltyWater(float grams_in_body, float volume_in_body) {
00126     
00127     float solution_gram_per_ten = 1.16f; // 1.0f for our solution
00128     float sol_volume = 10; // 10ml
00129     
00130     float ideal_ppt_constant = 0.007f; //7 ppt / 1000
00131     float x1 = volume_in_body * ideal_ppt_constant;
00132     float x2 = sol_volume * ideal_ppt_constant;
00133     
00134     x1 = x1 - grams_in_body;
00135     x2 = solution_gram_per_ten - x2;
00136     
00137     float output_ml = (x1 / x2 * sol_volume);
00138     
00139     return output_ml; // amount in ml to get 7 ppt.
00140 }
00141 
00142 bool PIDController::isHeating() {
00143     return this->heating;    
00144 }
00145 
00146 bool PIDController::isPumping() {
00147     return this->pumping;    
00148 }
00149 
00150 void PIDController::setHeating(bool enabled) {
00151     if(enabled == this->heating) return;
00152     
00153     this->heating = enabled;
00154     if(enabled) {
00155         #ifdef TEST_MODE
00156         printf("Should set heater to 1\r\n");
00157         #else
00158         heater = 1;
00159         #endif            
00160     } else {
00161         #ifdef TEST_MODE
00162         printf("Should set heater to 0\r\n");
00163         #else
00164         heater = 0;
00165         #endif    
00166     }
00167 }
00168 
00169 float PIDController::getMlFreshWater(float ppt, float volume) {
00170     return (volume * (ppt / 7.0)) - volume;
00171 }
00172 
00173 void PIDController::doTestingStuff(int ml) {
00174     while(1) {
00175         this->pumpSaltWater(ml);
00176         //this->pumpFreshWater(ml);
00177     }
00178 }
00179