Juan Salazar / robotic_fish_7

All mbed code for control over dive planes, pump motor, valve motor, BCUs, UART interface, etc.

Dependencies:   mbed ESC mbed MODDMA

robotic_fish_6/PumpWithValve/PumpWithValve.cpp@0:c3a329a5b05d, 2020-01-14 (annotated)

Committer:
juansal12
Date:
Tue Jan 14 19:17:05 2020 +0000
Revision:
0:c3a329a5b05d
Sofi7 mbed code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
juansal12 0:c3a329a5b05d 1 #include <PumpWithValve/PumpWithValve.h>
juansal12 0:c3a329a5b05d 2
juansal12 0:c3a329a5b05d 3 // The static instance
juansal12 0:c3a329a5b05d 4 PumpWithValve pumpWithValve;
juansal12 0:c3a329a5b05d 5
juansal12 0:c3a329a5b05d 6 void flipFlowUpStatic()
juansal12 0:c3a329a5b05d 7 {
juansal12 0:c3a329a5b05d 8 pumpWithValve.flipFlowUp();
juansal12 0:c3a329a5b05d 9 }
juansal12 0:c3a329a5b05d 10
juansal12 0:c3a329a5b05d 11 void flipFlowDownStatic()
juansal12 0:c3a329a5b05d 12 {
juansal12 0:c3a329a5b05d 13 pumpWithValve.flipFlowDown();
juansal12 0:c3a329a5b05d 14 }
juansal12 0:c3a329a5b05d 15
juansal12 0:c3a329a5b05d 16
juansal12 0:c3a329a5b05d 17 //============================================
juansal12 0:c3a329a5b05d 18 // Initialization
juansal12 0:c3a329a5b05d 19 //============================================
juansal12 0:c3a329a5b05d 20
juansal12 0:c3a329a5b05d 21 // Constructor
juansal12 0:c3a329a5b05d 22 PumpWithValve::PumpWithValve() :
juansal12 0:c3a329a5b05d 23 // Initialize variables
juansal12 0:c3a329a5b05d 24 pumpPWM(pumpPwmPin),
juansal12 0:c3a329a5b05d 25 valvePWM(valvePwmPin),
juansal12 0:c3a329a5b05d 26 //valveEncoder(encoderPinA, encoderPinB, NC, count2rev), // use X2 encoding by default
juansal12 0:c3a329a5b05d 27 //valveCurrent(valveCurrentPin),
juansal12 0:c3a329a5b05d 28 hallSignal(hallInterruptPin),
juansal12 0:c3a329a5b05d 29 valveLED(LED4)
juansal12 0:c3a329a5b05d 30 {
juansal12 0:c3a329a5b05d 31 hallSignal.rise(&flipFlowUpStatic);
juansal12 0:c3a329a5b05d 32 hallSignal.fall(&flipFlowDownStatic);
juansal12 0:c3a329a5b05d 33
juansal12 0:c3a329a5b05d 34 frequency = 0;
juansal12 0:c3a329a5b05d 35 thrust = 0;
juansal12 0:c3a329a5b05d 36 yaw = 0;
juansal12 0:c3a329a5b05d 37
juansal12 0:c3a329a5b05d 38 valveSide = true;
juansal12 0:c3a329a5b05d 39 valveV1 = 0;
juansal12 0:c3a329a5b05d 40 valveV2 = 0;
juansal12 0:c3a329a5b05d 41 Vfreq = 0;
juansal12 0:c3a329a5b05d 42 VfreqAdjusted = 0;
juansal12 0:c3a329a5b05d 43 Vset = 0;
juansal12 0:c3a329a5b05d 44 dVFreq = 0;
juansal12 0:c3a329a5b05d 45 freqErr = 0;
juansal12 0:c3a329a5b05d 46 prevFreqErr = 0;
juansal12 0:c3a329a5b05d 47
juansal12 0:c3a329a5b05d 48 timer.start();
juansal12 0:c3a329a5b05d 49 }
juansal12 0:c3a329a5b05d 50
juansal12 0:c3a329a5b05d 51
juansal12 0:c3a329a5b05d 52 void PumpWithValve::start()
juansal12 0:c3a329a5b05d 53 {
juansal12 0:c3a329a5b05d 54 valvePWM.write(0.0); // apply nothing to start
juansal12 0:c3a329a5b05d 55 pumpPWM.write(0.0);
juansal12 0:c3a329a5b05d 56 periodSide1 = 0.0;
juansal12 0:c3a329a5b05d 57 periodSide2 = 0.0;
juansal12 0:c3a329a5b05d 58
juansal12 0:c3a329a5b05d 59 timer.reset();
juansal12 0:c3a329a5b05d 60 valveControl.attach(&pumpWithValve, &PumpWithValve::setVoid, 0.08);
juansal12 0:c3a329a5b05d 61 }
juansal12 0:c3a329a5b05d 62
juansal12 0:c3a329a5b05d 63 void PumpWithValve::stop()
juansal12 0:c3a329a5b05d 64 {
juansal12 0:c3a329a5b05d 65 valveControl.detach();
juansal12 0:c3a329a5b05d 66 valvePWM.write(0.0);
juansal12 0:c3a329a5b05d 67 pumpPWM.write(0.0);
juansal12 0:c3a329a5b05d 68 }
juansal12 0:c3a329a5b05d 69
juansal12 0:c3a329a5b05d 70 void PumpWithValve::flipFlowUp()
juansal12 0:c3a329a5b05d 71 {
juansal12 0:c3a329a5b05d 72 // when the hall sensor sees a rising edge, we have rotated 180 degrees
juansal12 0:c3a329a5b05d 73 // --> want to adjust the applied voltage based on the side we are on
juansal12 0:c3a329a5b05d 74 valveSide = true; //change boolean state, keeps track of which half of the rotation we are on
juansal12 0:c3a329a5b05d 75 valveLED = 1;
juansal12 0:c3a329a5b05d 76 periodSide1 = timer.read_us();
juansal12 0:c3a329a5b05d 77 timer.reset();
juansal12 0:c3a329a5b05d 78 freqAct = 1/(periodSide1 + periodSide2);
juansal12 0:c3a329a5b05d 79 }
juansal12 0:c3a329a5b05d 80
juansal12 0:c3a329a5b05d 81 void PumpWithValve::flipFlowDown()
juansal12 0:c3a329a5b05d 82 {
juansal12 0:c3a329a5b05d 83 valveSide = false;
juansal12 0:c3a329a5b05d 84 valveLED = 0;
juansal12 0:c3a329a5b05d 85 periodSide2 = timer.read_us();
juansal12 0:c3a329a5b05d 86 timer.reset();
juansal12 0:c3a329a5b05d 87 freqAct = 1/(periodSide1 + periodSide2);
juansal12 0:c3a329a5b05d 88 }
juansal12 0:c3a329a5b05d 89
juansal12 0:c3a329a5b05d 90 //============================================
juansal12 0:c3a329a5b05d 91 // Processing
juansal12 0:c3a329a5b05d 92 //============================================
juansal12 0:c3a329a5b05d 93 void PumpWithValve::set(float freq_in, float yaw_in, float thrust_in){
juansal12 0:c3a329a5b05d 94 thrust = thrust_in;
juansal12 0:c3a329a5b05d 95 yaw = yaw_in;
juansal12 0:c3a329a5b05d 96 frequency = freq_in;
juansal12 0:c3a329a5b05d 97 }
juansal12 0:c3a329a5b05d 98
juansal12 0:c3a329a5b05d 99 void PumpWithValve::setVoid() {
juansal12 0:c3a329a5b05d 100 //Centrifugal Pump
juansal12 0:c3a329a5b05d 101 pumpPWM.write(thrust);
juansal12 0:c3a329a5b05d 102
juansal12 0:c3a329a5b05d 103 // set speed of the valve motor through the frequency value
juansal12 0:c3a329a5b05d 104 if (periodSide1 == 0 || periodSide2 == 0) {
juansal12 0:c3a329a5b05d 105 Vfreq = frequency * 400000; //just setting directly the voltage, scaled up; need to tune this value
juansal12 0:c3a329a5b05d 106 this->calculateYawMethod1();
juansal12 0:c3a329a5b05d 107 }
juansal12 0:c3a329a5b05d 108 else { // don't be fooled by initialization values
juansal12 0:c3a329a5b05d 109 // Failure mode - if it has been a full (desired) period since a hall sensor has been read
juansal12 0:c3a329a5b05d 110 if (timer.read_us() > 1.0 / frequency) {
juansal12 0:c3a329a5b05d 111 pumpWithValve.stop(); // may have to add a condition that allows for sudden input changes
juansal12 0:c3a329a5b05d 112 }
juansal12 0:c3a329a5b05d 113 else {
juansal12 0:c3a329a5b05d 114 freqErr = frequency - freqAct;
juansal12 0:c3a329a5b05d 115 dVFreq = KpFreq * freqErr + KdFreq * (freqErr - prevFreqErr);
juansal12 0:c3a329a5b05d 116 prevFreqErr = freqErr; //reset previous frequency error
juansal12 0:c3a329a5b05d 117 Vfreq += dVFreq;
juansal12 0:c3a329a5b05d 118 this->calculateYawMethod1();
juansal12 0:c3a329a5b05d 119 }
juansal12 0:c3a329a5b05d 120 }
juansal12 0:c3a329a5b05d 121 }
juansal12 0:c3a329a5b05d 122
juansal12 0:c3a329a5b05d 123
juansal12 0:c3a329a5b05d 124 void PumpWithValve::calculateYawMethod1()
juansal12 0:c3a329a5b05d 125 {
juansal12 0:c3a329a5b05d 126 // split tail frequency voltage into voltage on either side of the valve
juansal12 0:c3a329a5b05d 127 // TODO figure out ideal relationship between yaw and offf between V1 and V2
juansal12 0:c3a329a5b05d 128 // is it additive or multiplicative? start with super simple math
juansal12 0:c3a329a5b05d 129
juansal12 0:c3a329a5b05d 130 valveV1 = (1.0 + valveOffsetGain * yaw) * Vfreq;
juansal12 0:c3a329a5b05d 131 valveV2 = (1.0 - valveOffsetGain * yaw) * Vfreq;
juansal12 0:c3a329a5b05d 132
juansal12 0:c3a329a5b05d 133 // TODO need to decide whether to give up frequency or yaw when we are at input limits
juansal12 0:c3a329a5b05d 134 if (valveV1 > 1.0) {valveV1 = 1.0;}
juansal12 0:c3a329a5b05d 135 else if (valveV1 < 0.0) {valveV1 = 0.05;}
juansal12 0:c3a329a5b05d 136 if (valveV2 > 1.0) {valveV2 = 1.0;}
juansal12 0:c3a329a5b05d 137 else if (valveV2 < 0.0) {valveV2 = 0.05;}
juansal12 0:c3a329a5b05d 138
juansal12 0:c3a329a5b05d 139 // write valve voltage based on which side the hall sensor says we are on
juansal12 0:c3a329a5b05d 140 if (valveSide) { Vset = valveV1; }
juansal12 0:c3a329a5b05d 141 else { Vset = valveV2; }
juansal12 0:c3a329a5b05d 142
juansal12 0:c3a329a5b05d 143 valvePWM.write(Vset);
juansal12 0:c3a329a5b05d 144 }
juansal12 0:c3a329a5b05d 145
juansal12 0:c3a329a5b05d 146 void PumpWithValve::calculateYawMethod2()
juansal12 0:c3a329a5b05d 147 {
juansal12 0:c3a329a5b05d 148
juansal12 0:c3a329a5b05d 149 if (yaw < 0.0 && !valveSide) {
juansal12 0:c3a329a5b05d 150 Vset = (1.0 + valveOffsetGain*yaw)*Vfreq; // 0.7 can be adjusted to a power of 2 if needed
juansal12 0:c3a329a5b05d 151 if (Vset > 1.0) { VfreqAdjusted = 1.0; }
juansal12 0:c3a329a5b05d 152 }
juansal12 0:c3a329a5b05d 153 else if (yaw > 0.0 && valveSide) {
juansal12 0:c3a329a5b05d 154 Vset = (1.0 - valveOffsetGain*yaw)*Vfreq; // 0.7 can be adjusted to a power of 2 if needed
juansal12 0:c3a329a5b05d 155 if (Vset < 0.0) { VfreqAdjusted = 0.05; } // needs to keep turning
juansal12 0:c3a329a5b05d 156 }
juansal12 0:c3a329a5b05d 157 else {
juansal12 0:c3a329a5b05d 158 Vset = Vfreq;
juansal12 0:c3a329a5b05d 159 VfreqAdjusted = Vfreq;
juansal12 0:c3a329a5b05d 160 }
juansal12 0:c3a329a5b05d 161 valvePWM.write(VfreqAdjusted);
juansal12 0:c3a329a5b05d 162
juansal12 0:c3a329a5b05d 163 }
juansal12 0:c3a329a5b05d 164
juansal12 0:c3a329a5b05d 165 float PumpWithValve::getVset() { return Vset;}
juansal12 0:c3a329a5b05d 166 bool PumpWithValve::getVside() { return valveSide; }