Saltware / Mbed 2 deprecated Water Play

Program for the water play project for the course Software Testing Practical 2016 given at the VU University

Dependencies:   mbed DRV88255 TextLCD Ping mbed-rtos

PIDController.cpp@71:38bb6bf09933, 2016-06-21 (annotated)

Committer:
sbouber1
Date:
Tue Jun 21 12:09:12 2016 +0000
Revision:
71:38bb6bf09933
Parent:
70:642153418c6e
Child:
78:a44ad011dd41
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sbouber1 28:f4a4ee58d57e 1 #include "PIDController.h"
sbouber1 46:7e4c1f2ab76c 2 #include "LCDController.h"
sbouber1 35:c9261391a995 3
sbouber1 57:8dc3192ff150 4 static DigitalOut heater(p18);
sbouber1 28:f4a4ee58d57e 5
sbouber1 57:8dc3192ff150 6 static DRV8825 mtr_fresh(p21, p27, p28, p29, p22, p23);
sbouber1 57:8dc3192ff150 7 static DRV8825 mtr_salt(p24, p27, p28, p29, p25, p26);
joran 44:7c932cc5991b 8
joran 44:7c932cc5991b 9
sbouber1 28:f4a4ee58d57e 10 // This is called in the main loop on every iteration
sbouber1 28:f4a4ee58d57e 11 void PIDController::update() {
sbouber1 35:c9261391a995 12
sbouber1 57:8dc3192ff150 13 #ifndef TEST_MODE
sbouber1 38:930469a33001 14 if(this->num_iters < STARTUP_ITERATIONS) {
sbouber1 38:930469a33001 15 printf("PIDController: not running, startup phase");
sbouber1 38:930469a33001 16 return;
sbouber1 38:930469a33001 17 }
sbouber1 38:930469a33001 18 #endif
sbouber1 38:930469a33001 19
sbouber1 35:c9261391a995 20 // Control the heater
sbouber1 35:c9261391a995 21 // This could be done in the pumping function as well, if needed
joran 63:59335eb8bdbc 22 if (this->isHeating() && this->temp->getValue() >= TEMP_MIN_UNDESIRED + 1.0f) {
sbouber1 57:8dc3192ff150 23 this->setHeating(false);
joran 62:52cceb263591 24 } else if (!this->isHeating() && this->temp->getValue() < TEMP_MIN_UNDESIRED) {
sbouber1 57:8dc3192ff150 25 this->setHeating(true);
joran 49:ca6fb19fc280 26 }
sbouber1 35:c9261391a995 27
sbouber1 28:f4a4ee58d57e 28 float s = this->salt->getValue();
sbouber1 57:8dc3192ff150 29 float s_in_grams = this->getSaltInGrams();
sbouber1 30:cf12566013a5 30
sbouber1 65:b277b4067d4a 31 if(s < MIN_VALID_SALINITY) {
sbouber1 65:b277b4067d4a 32 cout << "PIDController: Salinity NC, no stabilizing action";
sbouber1 65:b277b4067d4a 33 } else if(s <= SALT_MIN_UNDESIRED) {
sbouber1 30:cf12566013a5 34
sbouber1 57:8dc3192ff150 35 float ml = this->getMlSaltyWater(s_in_grams, this->proximity->getValue());
sbouber1 70:642153418c6e 36 int ml_int = std::min(static_cast<int>(ml), 30);
sbouber1 70:642153418c6e 37 ml_int = std::max(1, ml_int);
sbouber1 30:cf12566013a5 38
sbouber1 65:b277b4067d4a 39 cout << "PIDCONTROLLER: need to pump " << ml_int << " (" << ml << ")" << " ml of salty water\r\n";
sbouber1 71:38bb6bf09933 40 printf("need to pump %d ml of salty water\r\n",ml_int);
sbouber1 30:cf12566013a5 41
sbouber1 71:38bb6bf09933 42 this->pumpSaltWater(ml_int);
sbouber1 30:cf12566013a5 43
joran 63:59335eb8bdbc 44 } else if(s >= SALT_MAX_UNDESIRED) {
sbouber1 32:1e4919a44196 45
sbouber1 32:1e4919a44196 46 float ml = this->getMlFreshWater(s, this->proximity->getValue());
sbouber1 70:642153418c6e 47 int ml_int = std::min(static_cast<int>(ml), 30);
sbouber1 70:642153418c6e 48 ml_int = std::max(1, ml_int);
sbouber1 32:1e4919a44196 49
sbouber1 65:b277b4067d4a 50 cout << "PIDCONTROLLER: need to pump " << ml_int << " (" << ml << ")" << " ml of fresh water\r\n";
sbouber1 71:38bb6bf09933 51 printf("need to pump %d ml of fresh water\r\n",ml_int);
sbouber1 35:c9261391a995 52
sbouber1 71:38bb6bf09933 53 this->pumpFreshWater(ml_int);
sbouber1 32:1e4919a44196 54
sbouber1 30:cf12566013a5 55 }
sbouber1 28:f4a4ee58d57e 56 }
sbouber1 28:f4a4ee58d57e 57
sbouber1 57:8dc3192ff150 58 std::string PIDController::getName() {
sbouber1 30:cf12566013a5 59 return "PIDController";
sbouber1 28:f4a4ee58d57e 60 }
sbouber1 28:f4a4ee58d57e 61
joran 29:2b256a7ce0ae 62 //get the salt in the body in grams
sbouber1 57:8dc3192ff150 63 float PIDController::getSaltInGrams() {
sbouber1 57:8dc3192ff150 64 float current_ppt = this->salt->getValue(); //in ppt
sbouber1 57:8dc3192ff150 65 float current_vol = this->proximity->getValue(); //in ml
joran 29:2b256a7ce0ae 66
sbouber1 57:8dc3192ff150 67 return current_ppt * (current_vol / 1000);
joran 29:2b256a7ce0ae 68 }
joran 29:2b256a7ce0ae 69
joran 29:2b256a7ce0ae 70
sbouber1 57:8dc3192ff150 71 void PIDController::pumpSaltWater(int ml) {
sbouber1 57:8dc3192ff150 72 this->pumpWater(&mtr_salt, ml);
sbouber1 32:1e4919a44196 73 }
sbouber1 32:1e4919a44196 74
sbouber1 32:1e4919a44196 75
sbouber1 57:8dc3192ff150 76 void PIDController::pumpFreshWater(int ml) {
sbouber1 57:8dc3192ff150 77 this->pumpWater(&mtr_fresh, ml);
sbouber1 32:1e4919a44196 78 }
sbouber1 32:1e4919a44196 79
sbouber1 57:8dc3192ff150 80
sbouber1 57:8dc3192ff150 81 void PIDController::pumpWater(DRV8825 *mtr, int ml) {
joran 44:7c932cc5991b 82 LCDController::showPumping();
sbouber1 57:8dc3192ff150 83
joran 40:1668630544c7 84 this->pumping = true;
sbouber1 57:8dc3192ff150 85
sbouber1 32:1e4919a44196 86 int j = 5010 * (ml - 1);
sbouber1 32:1e4919a44196 87
sbouber1 32:1e4919a44196 88 for (int i = 500; i < MAX_SPEED; i += 5) {
sbouber1 46:7e4c1f2ab76c 89 mtr->settings(1 / MICROSTEPS_PER_STEP, RIGHT, i);
sbouber1 46:7e4c1f2ab76c 90 }
sbouber1 46:7e4c1f2ab76c 91
sbouber1 46:7e4c1f2ab76c 92 for (int i = 0; i < 2010 + j; i += 1) {
sbouber1 46:7e4c1f2ab76c 93 mtr->settings(1 / MICROSTEPS_PER_STEP, RIGHT, 8000);
sbouber1 46:7e4c1f2ab76c 94 }
sbouber1 46:7e4c1f2ab76c 95
sbouber1 46:7e4c1f2ab76c 96 for (int i = 8000; i > 500; i -= 5) {
sbouber1 46:7e4c1f2ab76c 97 mtr->settings(1 / MICROSTEPS_PER_STEP, RIGHT, i);
sbouber1 46:7e4c1f2ab76c 98 }
sbouber1 46:7e4c1f2ab76c 99
sbouber1 46:7e4c1f2ab76c 100 wait(5);
sbouber1 46:7e4c1f2ab76c 101
sbouber1 46:7e4c1f2ab76c 102 for (int i = 500; i < MAX_SPEED; i += 5) {
sbouber1 32:1e4919a44196 103 mtr->settings(1 / MICROSTEPS_PER_STEP, LEFT, i);
sbouber1 32:1e4919a44196 104 }
sbouber1 32:1e4919a44196 105
sbouber1 32:1e4919a44196 106 for (int i = 0; i < 2010 + j; i += 1) {
sbouber1 32:1e4919a44196 107 mtr->settings(1 / MICROSTEPS_PER_STEP, LEFT, 8000);
sbouber1 32:1e4919a44196 108 }
sbouber1 32:1e4919a44196 109
sbouber1 32:1e4919a44196 110 for (int i = 8000; i > 500; i -= 5) {
sbouber1 32:1e4919a44196 111 mtr->settings(1 / MICROSTEPS_PER_STEP, LEFT, i);
sbouber1 32:1e4919a44196 112 }
sbouber1 32:1e4919a44196 113
sbouber1 46:7e4c1f2ab76c 114 wait(5);
joran 40:1668630544c7 115 this->pumping = false;
sbouber1 32:1e4919a44196 116 }
sbouber1 32:1e4919a44196 117
sbouber1 32:1e4919a44196 118
sbouber1 57:8dc3192ff150 119 float PIDController::getMlSaltyWater(float grams_in_body, float volume_in_body) {
sbouber1 32:1e4919a44196 120
sbouber1 57:8dc3192ff150 121 float solution_gram_per_ten = 1.16f; // 1.0f for our solution
sbouber1 57:8dc3192ff150 122 float sol_volume = 10; // 10ml
sbouber1 32:1e4919a44196 123
sbouber1 57:8dc3192ff150 124 float ideal_ppt_constant = 0.007f; //7 ppt / 1000
sbouber1 57:8dc3192ff150 125 float x1 = volume_in_body * ideal_ppt_constant;
sbouber1 57:8dc3192ff150 126 float x2 = sol_volume * ideal_ppt_constant;
sbouber1 32:1e4919a44196 127
sbouber1 57:8dc3192ff150 128 x1 = x1 - grams_in_body;
sbouber1 57:8dc3192ff150 129 x2 = solution_gram_per_ten - x2;
sbouber1 32:1e4919a44196 130
sbouber1 57:8dc3192ff150 131 float output_ml = (x1 / x2 * sol_volume);
sbouber1 32:1e4919a44196 132
sbouber1 57:8dc3192ff150 133 return output_ml; // amount in ml to get 7 ppt.
sbouber1 35:c9261391a995 134 }
sbouber1 35:c9261391a995 135
sbouber1 57:8dc3192ff150 136 bool PIDController::isHeating() {
sbouber1 35:c9261391a995 137 return this->heating;
sbouber1 35:c9261391a995 138 }
sbouber1 35:c9261391a995 139
sbouber1 57:8dc3192ff150 140 bool PIDController::isPumping() {
joran 40:1668630544c7 141 return this->pumping;
joran 40:1668630544c7 142 }
joran 40:1668630544c7 143
sbouber1 57:8dc3192ff150 144 void PIDController::setHeating(bool enabled) {
sbouber1 35:c9261391a995 145 if(enabled == this->heating) return;
sbouber1 35:c9261391a995 146
sbouber1 35:c9261391a995 147 this->heating = enabled;
sbouber1 35:c9261391a995 148 if(enabled) {
sbouber1 57:8dc3192ff150 149 #ifdef TEST_MODE
sbouber1 35:c9261391a995 150 printf("Should set heater to 1\r\n");
sbouber1 35:c9261391a995 151 #else
sbouber1 35:c9261391a995 152 heater = 1;
sbouber1 35:c9261391a995 153 #endif
sbouber1 35:c9261391a995 154 } else {
sbouber1 57:8dc3192ff150 155 #ifdef TEST_MODE
sbouber1 35:c9261391a995 156 printf("Should set heater to 0\r\n");
sbouber1 35:c9261391a995 157 #else
sbouber1 35:c9261391a995 158 heater = 0;
sbouber1 35:c9261391a995 159 #endif
sbouber1 32:1e4919a44196 160 }
sbouber1 35:c9261391a995 161 }
sbouber1 32:1e4919a44196 162
sbouber1 32:1e4919a44196 163 float PIDController::getMlFreshWater(float ppt, float volume) {
sbouber1 32:1e4919a44196 164 return (volume * (ppt / 7.0)) - volume;
sbouber1 32:1e4919a44196 165 }
sbouber1 32:1e4919a44196 166
sbouber1 46:7e4c1f2ab76c 167 void PIDController::doTestingStuff(int ml) {
sbouber1 71:38bb6bf09933 168 while(1) {
sbouber1 57:8dc3192ff150 169 this->pumpSaltWater(ml);
sbouber1 71:38bb6bf09933 170 //this->pumpFreshWater(ml);
sbouber1 71:38bb6bf09933 171 }
sbouber1 46:7e4c1f2ab76c 172 }
sbouber1 46:7e4c1f2ab76c 173