Qubit 2020 / presensfirmwareupdate

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

2021fork3/pump.cpp@2:4603eae79fb6, 2022-06-23 (annotated)

Committer:
whiteheadrf
Date:
Thu Jun 23 19:33:33 2022 +0000
Revision:
2:4603eae79fb6
Parent:
1:eac9f7a0488f 
version for xbee wifi, new presens boards with firmware update to include view0005 and 4 wire pumps

Who changed what in which revision?

UserRevisionLine numberNew contents of line
whiteheadrf 0:1121a7ef2e36 1 #include "mbed.h"
whiteheadrf 0:1121a7ef2e36 2 #include "pump.h"
whiteheadrf 0:1121a7ef2e36 3 #include "light.h"
whiteheadrf 0:1121a7ef2e36 4 #include "debug.h"
whiteheadrf 0:1121a7ef2e36 5 #include "wifi_events.h"
whiteheadrf 0:1121a7ef2e36 6
whiteheadrf 0:1121a7ef2e36 7 // According to specification for DM212 pump PWM period is 15 ms. new pump driver period 4-2019
whiteheadrf 0:1121a7ef2e36 8 #define PUMP_PWM_PERIOD_US 15000
whiteheadrf 0:1121a7ef2e36 9
whiteheadrf 0:1121a7ef2e36 10 // Pulse width for DM212 pump can be 1-2 ms,
whiteheadrf 0:1121a7ef2e36 11 // where 1 ms - pump is off, 2 ms - pump is on with maximum speed. new pump driver parameters 4-2019
whiteheadrf 2:4603eae79fb6 12 #define PUMP_PULSE_WIDTH_MIN_US 950
whiteheadrf 0:1121a7ef2e36 13 #define PUMP_PULSE_WIDTH_MAX_US 2000
whiteheadrf 0:1121a7ef2e36 14
whiteheadrf 0:1121a7ef2e36 15 // Zero pulse width is used to disable pulse
whiteheadrf 0:1121a7ef2e36 16 #define PUMP_DISABLE_PULSE 0
whiteheadrf 0:1121a7ef2e36 17
whiteheadrf 0:1121a7ef2e36 18 // Time to wait to be sure that signal sent 3 times.
whiteheadrf 0:1121a7ef2e36 19 // Measured during experiments, can be tuned in the future.
whiteheadrf 0:1121a7ef2e36 20 #define PUMP_DELAY_MS 500
whiteheadrf 0:1121a7ef2e36 21
whiteheadrf 0:1121a7ef2e36 22 // File with pump parameters
whiteheadrf 0:1121a7ef2e36 23 #define PUMP_FILE "/" FSNAME "/Parameters/pump.sys"
whiteheadrf 0:1121a7ef2e36 24
whiteheadrf 0:1121a7ef2e36 25 // Measurements number for pump feedback
whiteheadrf 0:1121a7ef2e36 26 #define PUMP_FEEDBACK_NUM_MEASUREMENTS 10
whiteheadrf 0:1121a7ef2e36 27
whiteheadrf 0:1121a7ef2e36 28 // Maximum period for RPM feedback
whiteheadrf 0:1121a7ef2e36 29 #define PUMP_FEEDBACK_MAX_PERIOD 0.5
whiteheadrf 0:1121a7ef2e36 30
whiteheadrf 0:1121a7ef2e36 31 // Pump polynom coefficients
whiteheadrf 0:1121a7ef2e36 32 // Default values for ideal pump (linear dependency)
whiteheadrf 0:1121a7ef2e36 33 static float a = -50.0;
whiteheadrf 0:1121a7ef2e36 34 static float b = 0.02;
whiteheadrf 0:1121a7ef2e36 35 static float c = 0.0;
whiteheadrf 0:1121a7ef2e36 36
whiteheadrf 0:1121a7ef2e36 37 // PWM out to handle pump
whiteheadrf 0:1121a7ef2e36 38 static PwmOut pumpPwm(p26);
whiteheadrf 0:1121a7ef2e36 39
whiteheadrf 0:1121a7ef2e36 40 // Feedback timer
whiteheadrf 0:1121a7ef2e36 41 static Timer feedbackTimer;
whiteheadrf 0:1121a7ef2e36 42
whiteheadrf 0:1121a7ef2e36 43 // Feedback interrupt
whiteheadrf 0:1121a7ef2e36 44 static InterruptIn feedbackInterrupt(p18);
whiteheadrf 0:1121a7ef2e36 45
whiteheadrf 0:1121a7ef2e36 46 // Latest applied pump PercentPump
whiteheadrf 0:1121a7ef2e36 47 static float lastPercentPump;
whiteheadrf 0:1121a7ef2e36 48
whiteheadrf 0:1121a7ef2e36 49
whiteheadrf 0:1121a7ef2e36 50 // Latest average feedback period
whiteheadrf 0:1121a7ef2e36 51 static float feedbackPeriod;
whiteheadrf 0:1121a7ef2e36 52
whiteheadrf 0:1121a7ef2e36 53 static void feedbackRise(void)
whiteheadrf 0:1121a7ef2e36 54 {
whiteheadrf 0:1121a7ef2e36 55 static float periodSum = 0;
whiteheadrf 0:1121a7ef2e36 56 static int numMeasureaments = 0;
whiteheadrf 0:1121a7ef2e36 57
whiteheadrf 0:1121a7ef2e36 58 // Read period
whiteheadrf 0:1121a7ef2e36 59 float period = feedbackTimer.read();
whiteheadrf 0:1121a7ef2e36 60 feedbackTimer.reset();
whiteheadrf 0:1121a7ef2e36 61
whiteheadrf 0:1121a7ef2e36 62 if (period > PUMP_FEEDBACK_MAX_PERIOD) {
whiteheadrf 0:1121a7ef2e36 63 periodSum = 0;
whiteheadrf 0:1121a7ef2e36 64 numMeasureaments = 0;
whiteheadrf 0:1121a7ef2e36 65 feedbackPeriod = 0;
whiteheadrf 0:1121a7ef2e36 66 return;
whiteheadrf 0:1121a7ef2e36 67 }
whiteheadrf 0:1121a7ef2e36 68
whiteheadrf 0:1121a7ef2e36 69 periodSum += period;
whiteheadrf 0:1121a7ef2e36 70 numMeasureaments++;
whiteheadrf 0:1121a7ef2e36 71
whiteheadrf 0:1121a7ef2e36 72 if (numMeasureaments >= PUMP_FEEDBACK_NUM_MEASUREMENTS) {
whiteheadrf 0:1121a7ef2e36 73 feedbackPeriod = periodSum / numMeasureaments;
whiteheadrf 0:1121a7ef2e36 74 periodSum = 0;
whiteheadrf 0:1121a7ef2e36 75 numMeasureaments = 0;
whiteheadrf 0:1121a7ef2e36 76 }
whiteheadrf 0:1121a7ef2e36 77 }
whiteheadrf 0:1121a7ef2e36 78
whiteheadrf 0:1121a7ef2e36 79 void pumpInit(void)
whiteheadrf 0:1121a7ef2e36 80 {
whiteheadrf 0:1121a7ef2e36 81 // Configure PWM period
whiteheadrf 0:1121a7ef2e36 82 pumpPwm.period_us(PUMP_PWM_PERIOD_US);
whiteheadrf 0:1121a7ef2e36 83
whiteheadrf 0:1121a7ef2e36 84 // Turn pump off
whiteheadrf 0:1121a7ef2e36 85 pumpPwm.pulsewidth_us(PUMP_PULSE_WIDTH_MIN_US);
whiteheadrf 0:1121a7ef2e36 86
whiteheadrf 0:1121a7ef2e36 87 // Wait to be sure that signal sent 3 times.
whiteheadrf 0:1121a7ef2e36 88 wait_ms(PUMP_DELAY_MS);
whiteheadrf 0:1121a7ef2e36 89
whiteheadrf 0:1121a7ef2e36 90 // Disable pump pulse.
whiteheadrf 0:1121a7ef2e36 91 pumpPwm.pulsewidth_us(PUMP_DISABLE_PULSE);
whiteheadrf 0:1121a7ef2e36 92
whiteheadrf 0:1121a7ef2e36 93
whiteheadrf 0:1121a7ef2e36 94 // Read parameters
whiteheadrf 0:1121a7ef2e36 95 FILE* paramFile = fopen(PUMP_FILE, "r");
whiteheadrf 0:1121a7ef2e36 96 if (paramFile != NULL) {
whiteheadrf 0:1121a7ef2e36 97 fscanf(paramFile, "%f,%f,%f", &a, &b, &c);
whiteheadrf 0:1121a7ef2e36 98 fclose(paramFile);
whiteheadrf 0:1121a7ef2e36 99 }
whiteheadrf 0:1121a7ef2e36 100
whiteheadrf 0:1121a7ef2e36 101 INFO("Used pump coefficients: a=%f, b=%0.6f, c=%0.9f", a, b, c);
whiteheadrf 0:1121a7ef2e36 102
whiteheadrf 0:1121a7ef2e36 103 // Set callback for feedback
whiteheadrf 0:1121a7ef2e36 104 feedbackInterrupt.rise(&feedbackRise);
whiteheadrf 0:1121a7ef2e36 105 feedbackTimer.start();
whiteheadrf 0:1121a7ef2e36 106 }
whiteheadrf 0:1121a7ef2e36 107
whiteheadrf 0:1121a7ef2e36 108 void pumpSet(float PercentPump)
whiteheadrf 0:1121a7ef2e36 109 {
whiteheadrf 0:1121a7ef2e36 110 if (PercentPump > PUMP_MAX_INTENSITY) {
whiteheadrf 0:1121a7ef2e36 111 // Don't support PercentPump more than 100%
whiteheadrf 0:1121a7ef2e36 112 ERROR("Request to set too big pump PercentPump %f", PercentPump);
whiteheadrf 0:1121a7ef2e36 113 return;
whiteheadrf 0:1121a7ef2e36 114 }
whiteheadrf 0:1121a7ef2e36 115
whiteheadrf 1:eac9f7a0488f 116 // if (lastPercentPump == PercentPump) {
whiteheadrf 1:eac9f7a0488f 117 // INFO("Discard request to set pump PercentPump, PercentPump %f already applied", PercentPump);
whiteheadrf 1:eac9f7a0488f 118 // return;
whiteheadrf 1:eac9f7a0488f 119 // }
whiteheadrf 0:1121a7ef2e36 120
whiteheadrf 0:1121a7ef2e36 121 lastPercentPump = PercentPump;
whiteheadrf 0:1121a7ef2e36 122
whiteheadrf 0:1121a7ef2e36 123
whiteheadrf 0:1121a7ef2e36 124 // As PWM interface for LPC1768 uses the same period for all PWM outs
whiteheadrf 0:1121a7ef2e36 125 // read lignt intensity, turn light off and set it back when pump operation finished.
whiteheadrf 0:1121a7ef2e36 126 unsigned char lightIntensity = lightRead();
whiteheadrf 0:1121a7ef2e36 127
whiteheadrf 0:1121a7ef2e36 128 // Configure PWM period and set pulse for pump.
whiteheadrf 0:1121a7ef2e36 129 pumpPwm.period_us(PUMP_PWM_PERIOD_US);
whiteheadrf 0:1121a7ef2e36 130
whiteheadrf 0:1121a7ef2e36 131 // Set light off
whiteheadrf 0:1121a7ef2e36 132 lightSet(LIGHT_OFF_INTENSITY);
whiteheadrf 0:1121a7ef2e36 133
whiteheadrf 0:1121a7ef2e36 134 int pulseWidth = PUMP_PULSE_WIDTH_MIN_US +
whiteheadrf 0:1121a7ef2e36 135 (PUMP_PULSE_WIDTH_MAX_US - PUMP_PULSE_WIDTH_MIN_US) * PercentPump / PUMP_MAX_INTENSITY;
whiteheadrf 0:1121a7ef2e36 136 pumpPwm.pulsewidth_us(pulseWidth);
whiteheadrf 0:1121a7ef2e36 137
whiteheadrf 0:1121a7ef2e36 138 // Wait to be sure that signal sent 3 times.
whiteheadrf 0:1121a7ef2e36 139 wait_ms(PUMP_DELAY_MS);
whiteheadrf 0:1121a7ef2e36 140
whiteheadrf 0:1121a7ef2e36 141 // Disable pump pulse.
whiteheadrf 0:1121a7ef2e36 142 pumpPwm.pulsewidth_us(PUMP_DISABLE_PULSE);
whiteheadrf 0:1121a7ef2e36 143
whiteheadrf 0:1121a7ef2e36 144 // Return light intensity back.
whiteheadrf 0:1121a7ef2e36 145 lightSet(lightIntensity);
whiteheadrf 0:1121a7ef2e36 146 }
whiteheadrf 0:1121a7ef2e36 147
whiteheadrf 0:1121a7ef2e36 148 void pumpSetRpm(int rpm)
whiteheadrf 0:1121a7ef2e36 149 {
whiteheadrf 0:1121a7ef2e36 150 // Calculate PercentPump according to provided RPM
whiteheadrf 0:1121a7ef2e36 151 float PercentPump = a + b * rpm + c * rpm * rpm;
whiteheadrf 0:1121a7ef2e36 152
whiteheadrf 0:1121a7ef2e36 153 if (PercentPump < PUMP_MIN_INTENSITY) {
whiteheadrf 0:1121a7ef2e36 154 PercentPump = PUMP_OFF_INTENSITY;
whiteheadrf 0:1121a7ef2e36 155 }
whiteheadrf 0:1121a7ef2e36 156 if (PercentPump > PUMP_MAX_INTENSITY) {
whiteheadrf 0:1121a7ef2e36 157 PercentPump = PUMP_MAX_INTENSITY;
whiteheadrf 0:1121a7ef2e36 158 }
whiteheadrf 0:1121a7ef2e36 159
whiteheadrf 0:1121a7ef2e36 160 DEBUG1("Set pump PercentPump: %f", PercentPump);
whiteheadrf 0:1121a7ef2e36 161 pumpSet(PercentPump);
whiteheadrf 0:1121a7ef2e36 162 }
whiteheadrf 0:1121a7ef2e36 163
whiteheadrf 0:1121a7ef2e36 164 bool pumpOn(void)
whiteheadrf 0:1121a7ef2e36 165 {
whiteheadrf 0:1121a7ef2e36 166 return (lastPercentPump > PUMP_OFF_INTENSITY);
whiteheadrf 0:1121a7ef2e36 167 }
whiteheadrf 0:1121a7ef2e36 168
whiteheadrf 0:1121a7ef2e36 169 unsigned char pumpCurrentIntensity(void)
whiteheadrf 0:1121a7ef2e36 170 {
whiteheadrf 0:1121a7ef2e36 171 return lastPercentPump;
whiteheadrf 0:1121a7ef2e36 172 }
whiteheadrf 0:1121a7ef2e36 173
whiteheadrf 0:1121a7ef2e36 174 unsigned int pumpSpeed(void)
whiteheadrf 0:1121a7ef2e36 175 {
whiteheadrf 0:1121a7ef2e36 176 // Calculate pump speed
whiteheadrf 0:1121a7ef2e36 177 unsigned int speed = 0;
whiteheadrf 0:1121a7ef2e36 178
whiteheadrf 0:1121a7ef2e36 179 if (pumpCurrentIntensity() == 0) {
whiteheadrf 0:1121a7ef2e36 180 return 0;
whiteheadrf 0:1121a7ef2e36 181 }
whiteheadrf 0:1121a7ef2e36 182
whiteheadrf 0:1121a7ef2e36 183 if (feedbackPeriod > 0.0) {
whiteheadrf 0:1121a7ef2e36 184 speed = 60 / feedbackPeriod;
whiteheadrf 0:1121a7ef2e36 185
whiteheadrf 0:1121a7ef2e36 186 }
whiteheadrf 0:1121a7ef2e36 187 DEBUG1("Pump speed: %u", speed);
whiteheadrf 0:1121a7ef2e36 188
whiteheadrf 0:1121a7ef2e36 189 static bool pumpErrorSent = false;
whiteheadrf 0:1121a7ef2e36 190 if (speed != 0) {
whiteheadrf 0:1121a7ef2e36 191 pumpErrorSent = false;
whiteheadrf 0:1121a7ef2e36 192 } else if (!pumpErrorSent) {
whiteheadrf 0:1121a7ef2e36 193 wifiEventsSendErrorsInd(ERROR_PUMP_RPM_NULL);
whiteheadrf 0:1121a7ef2e36 194 pumpErrorSent = true;
whiteheadrf 0:1121a7ef2e36 195 }
whiteheadrf 0:1121a7ef2e36 196
whiteheadrf 0:1121a7ef2e36 197 return speed;
whiteheadrf 0:1121a7ef2e36 198 }
whiteheadrf 0:1121a7ef2e36 199
whiteheadrf 0:1121a7ef2e36 200 void pumpGetParams(float* aValue, float* bValue, float* cValue)
whiteheadrf 0:1121a7ef2e36 201 {
whiteheadrf 0:1121a7ef2e36 202 *aValue = a;
whiteheadrf 0:1121a7ef2e36 203 *bValue = b;
whiteheadrf 0:1121a7ef2e36 204 *cValue = c;
whiteheadrf 0:1121a7ef2e36 205 }
whiteheadrf 0:1121a7ef2e36 206
whiteheadrf 0:1121a7ef2e36 207 void pumpSetParams(float aValue, float bValue, float cValue)
whiteheadrf 0:1121a7ef2e36 208 {
whiteheadrf 0:1121a7ef2e36 209 a = aValue;
whiteheadrf 0:1121a7ef2e36 210 b = bValue;
whiteheadrf 0:1121a7ef2e36 211 c = cValue;
whiteheadrf 0:1121a7ef2e36 212
whiteheadrf 0:1121a7ef2e36 213 FILE* paramFile = fopen(PUMP_FILE, "w");
whiteheadrf 0:1121a7ef2e36 214 fprintf(paramFile, "%f,%0.6f,%0.9f", a, b, c);
whiteheadrf 0:1121a7ef2e36 215 fclose(paramFile);
whiteheadrf 0:1121a7ef2e36 216 }