PumpWithValve.cpp

00001 #include <PumpWithValve/PumpWithValve.h>
00002 
00003 // The static instance
00004 PumpWithValve pumpWithValve;
00005 
00006 void flipFlowUpStatic()
00007 {
00008     pumpWithValve.flipFlowUp();
00009 }
00010 
00011 void flipFlowDownStatic()
00012 {
00013     pumpWithValve.flipFlowDown();
00014 }
00015 
00016 
00017 //============================================
00018 // Initialization
00019 //============================================
00020 
00021 // Constructor
00022 PumpWithValve::PumpWithValve() :
00023             // Initialize variables
00024             pumpPWM(pumpPwmPin),
00025             valvePWM(valvePwmPin),
00026             //valveEncoder(encoderPinA, encoderPinB, NC, count2rev), // use X2 encoding by default
00027             //valveCurrent(valveCurrentPin),
00028             hallSignal(hallInterruptPin),
00029             valveLED(LED4)
00030 {
00031     hallSignal.rise(&flipFlowUpStatic);
00032     hallSignal.fall(&flipFlowDownStatic);
00033 
00034     frequency = 0;
00035     thrust = 0;
00036     yaw = 0;
00037 
00038     valveSide = true;
00039     valveV1 = 0;
00040     valveV2 = 0;
00041     Vfreq = 0;
00042     VfreqAdjusted = 0;
00043     Vset = 0;
00044     dVFreq = 0;
00045     freqErr = 0;
00046     prevFreqErr = 0;
00047 
00048     timer.start();
00049 }
00050 
00051 
00052 void PumpWithValve::start()
00053 {
00054     valvePWM.write(0.0); // apply nothing to start
00055     pumpPWM.write(0.0);
00056     periodSide1 = 0.0;
00057     periodSide2 = 0.0;
00058 
00059     timer.reset();
00060     valveControl.attach(&pumpWithValve, &PumpWithValve::setVoid, 0.08);
00061 }
00062 
00063 void PumpWithValve::stop()
00064 {
00065     valveControl.detach();
00066     valvePWM.write(0.0);
00067     pumpPWM.write(0.0);
00068 }
00069 
00070 void PumpWithValve::flipFlowUp()
00071 {
00072     // when the hall sensor sees a rising edge, we have rotated 180 degrees
00073     // --> want to adjust the applied voltage based on the side we are on
00074     valveSide = true; //change boolean state, keeps track of which half of the rotation we are on
00075     valveLED = 1;
00076     periodSide1 = timer.read_us();
00077     timer.reset();
00078     freqAct = 1/(periodSide1 + periodSide2);
00079 }
00080 
00081 void PumpWithValve::flipFlowDown()
00082 {
00083     valveSide = false;
00084     valveLED = 0;
00085     periodSide2 = timer.read_us();
00086     timer.reset();
00087     freqAct = 1/(periodSide1 + periodSide2);
00088 }
00089 
00090 //============================================
00091 // Processing
00092 //============================================
00093 void PumpWithValve::set(float freq_in, float yaw_in, float thrust_in){
00094     thrust = thrust_in;
00095     yaw = yaw_in;
00096     frequency = freq_in;
00097 }
00098 
00099 void PumpWithValve::setVoid() {
00100     //Centrifugal Pump
00101     pumpPWM.write(thrust);
00102 
00103     // set speed of the valve motor through the frequency value
00104     if (periodSide1 == 0 || periodSide2 == 0) {
00105         Vfreq = frequency * 400000; //just setting directly the voltage, scaled up; need to tune this value
00106         this->calculateYawMethod1();
00107     }
00108     else { // don't be fooled by initialization values
00109            // Failure mode - if it has been a full (desired) period since a hall sensor has been read
00110         if (timer.read_us() > 1.0 / frequency) {
00111             pumpWithValve.stop(); // may have to add a condition that allows for sudden input changes
00112         }
00113         else {
00114             freqErr = frequency - freqAct;
00115             dVFreq = KpFreq * freqErr + KdFreq * (freqErr - prevFreqErr);
00116             prevFreqErr = freqErr; //reset previous frequency error
00117             Vfreq += dVFreq;
00118             this->calculateYawMethod1();
00119         }
00120     }
00121 }
00122 
00123 
00124 void PumpWithValve::calculateYawMethod1()
00125 {
00126     // split tail frequency voltage into voltage on either side of the valve
00127     // TODO figure out ideal relationship between yaw and offf between V1 and V2
00128     // is it additive or multiplicative? start with super simple math
00129 
00130     valveV1 = (1.0 + valveOffsetGain * yaw) * Vfreq;
00131     valveV2 = (1.0 - valveOffsetGain * yaw) * Vfreq;
00132 
00133     // TODO need to decide whether to give up frequency or yaw when we are at input limits
00134     if (valveV1 > 1.0) {valveV1 = 1.0;}
00135     else if (valveV1 < 0.0) {valveV1 = 0.05;}
00136     if (valveV2 > 1.0) {valveV2 = 1.0;}
00137     else if (valveV2 < 0.0) {valveV2 = 0.05;}
00138 
00139     // write valve voltage based on which side the hall sensor says we are on
00140     if (valveSide) { Vset = valveV1; }
00141     else { Vset = valveV2; }
00142 
00143     valvePWM.write(Vset);
00144 }
00145 
00146 void PumpWithValve::calculateYawMethod2()
00147 {
00148 
00149     if (yaw < 0.0 && !valveSide) {
00150         Vset = (1.0 + valveOffsetGain*yaw)*Vfreq; // 0.7 can be adjusted to a power of 2 if needed
00151         if (Vset > 1.0) { VfreqAdjusted = 1.0; }
00152     } 
00153     else if (yaw > 0.0 && valveSide) {
00154         Vset = (1.0 - valveOffsetGain*yaw)*Vfreq; // 0.7 can be adjusted to a power of 2 if needed
00155         if (Vset < 0.0) { VfreqAdjusted = 0.05; } // needs to keep turning
00156     }
00157     else {
00158         Vset = Vfreq;
00159         VfreqAdjusted = Vfreq;
00160     }
00161     valvePWM.write(VfreqAdjusted);
00162 
00163 }
00164 
00165 float PumpWithValve::getVset() { return Vset;}
00166 bool PumpWithValve::getVside() { return valveSide; }