using System; using System.IO.Ports; using System.Text; using System.Threading; using Microsoft.SPOT.Hardware; using SWModules; namespace HWModules { public sealed class BuoyancyEngine { private MotherBoard mb; private ADC2485 bellowsPositionADC; private ElmoTwitter pumpMotor; private BuoyancyEngine() { mb = MotherBoard.Instance; bellowsPositionADC = new ADC2485(new I2CDevice.Configuration(0x26, 100), 6); pumpMotor = new ElmoTwitter("PumpMotor", "COM2", 115200, Parity.None, 8, StopBits.One, 5000, 20, false, (byte)';', (byte)';'); } private static BuoyancyEngine instance; public static BuoyancyEngine Instance { get { instance = instance ?? (new BuoyancyEngine()); return instance; } } public static StringBuilder sbTemp = new StringBuilder(128); public static double IB { get; private set; } public static double OB { get; private set; } public void init() { IB = 0.75; OB = 2.0; pumpMotor.init(); stopMotor(); closeMV(); } private const double sensorLength = 250.1; public double bellowsPosition { get; private set; } public double getBellowsPositionMTS() { int value = 0; double volts = double.NaN; double voltage = double.NaN; value = bellowsPositionADC.readValue(); if (bellowsPosition != int.MaxValue) { value = (int)(value ^ 0x80000000); //Need to invert the msb per 2485 data sheet //Debug.Print("ADC Value = " + value.ToString()); volts = (float)value; voltage = 2.5 + (5.0 * (double)volts / 2147483648.0); //Debug.Print("Voltage = " + voltage.ToString() + "\r\n"); bellowsPosition = (voltage - 0.1) * (sensorLength / 4.8); //Debug.Print("Position = " + position.ToString()); } if (missionConfig.verbosityLevel > 5) EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, DateTime.Now, EngrLogger.ColumnNums.state, SV.CurrentState, EngrLogger.ColumnNums.comment, "Bellows Position", EngrLogger.ColumnNums.bellowsPosition, bellowsPosition); return (bellowsPosition); } public double getBellowsPositionStringPot() { int value = 0; double volts = double.NaN; double voltage = double.NaN; const double fullScaleStringOut = 500.0; //mm const double fullScaleVolts = 2.5; const double voltageOffset = -2.5; const double MMperVolt = fullScaleStringOut / (fullScaleVolts - voltageOffset); value = bellowsPositionADC.readValue(); if (bellowsPosition != int.MaxValue) { //Convert ADC value to volts per 2485 data sheet value = (int)(value ^ 0x80000000); //Debug.Print("ADC Value = " + value.ToString()); volts = (float)value; voltage = 5.0 * (double)volts / 2147483648.0; //Debug.Print("Voltage = " + voltage.ToString() + "\r\n"); //Convert voltage to position bellowsPosition = (voltage - voltageOffset) * MMperVolt; //Debug.Print("Position = " + bellowsPosition.ToString()); if (missionConfig.verbosityLevel > 5) EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, DateTime.Now, EngrLogger.ColumnNums.state, SV.CurrentState, EngrLogger.ColumnNums.comment, "Bellows Position", EngrLogger.ColumnNums.bellowsPosition, bellowsPosition); } return (bellowsPosition); } public void processMessage(QRecord qRecord) { } public bool motorStopped = false; public void stopMotor() { pumpMotor.stopMotor(); increasingBuoyancy = false; decreasingBuoyancy = false; motorStopped = true; } private static int exerciseBellowsJV = 30000; public int exerciseBellows() { double bellowsPosition = double.NaN; sbTemp.Clear(); sbTemp.Append("Increasing bellows position"); Thread.Sleep(1000); decreaseBuoyancy(exerciseBellowsJV); for (int i = 0; i < 10; i++) { //CN0254Volts = CN0254.scanAll(); //bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset; bellowsPosition = getBellowsPositionStringPot(); EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, DateTime.Now, EngrLogger.ColumnNums.state, SV.CurrentState, EngrLogger.ColumnNums.comment, sbTemp, EngrLogger.ColumnNums.bellowsPosition, bellowsPosition); Thread.Sleep(2000); } pumpMotor.stopMotor(); Thread.Sleep(200); sbTemp.Clear(); sbTemp.Append("Decreasing bellows position"); increaseBuoyancy(exerciseBellowsJV); for (int i = 0; i < 10; i++) { bellowsPosition = getBellowsPositionStringPot(); EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, DateTime.Now, EngrLogger.ColumnNums.state, SV.CurrentState, EngrLogger.ColumnNums.comment, sbTemp, EngrLogger.ColumnNums.bellowsPosition, bellowsPosition); Thread.Sleep(2000); } stopMotor(); return (1); } private static int moveBellowsInsideLimitsJV = 30000; public int moveBellowsInsideLimits() { //Just in case the bellows was outside the limits, put it inside the limits bellowsPosition = getBellowsPositionStringPot(); if (bellowsPosition > (missionConfig.bellowsUpperLimit - 1.0)) { sbTemp.Clear(); sbTemp.Append("Inner bellows exceeds upper limit, decreasing bellows position"); EngrLogger.writeToColumns(sbTemp); if (!decreasingBuoyancy) increaseBuoyancy(moveBellowsInsideLimitsJV); return (0); //action not complete, return 0 } else if (bellowsPosition < (missionConfig.bellowsLowerLimit + 1.0)) { sbTemp.Clear(); sbTemp.Append("Inner bellows below lower limit, increasing bellows position"); EngrLogger.writeToColumns(sbTemp); if (!increasingBuoyancy) decreaseBuoyancy(moveBellowsInsideLimitsJV); return (0); //action not complete, return 0 } else { stopMotor(); sbTemp.Clear(); sbTemp.Append("Bellows inside limits"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return (1); } } public bool increasingBuoyancy = false; public bool increaseBuoyancy(int jogVelocityCPS) { if (!SV.BellowsAtLowerLimit) { if (!isValveOpen()) { openMV(); Thread.Sleep(1500); } pumpMotor.sendJV(System.Math.Abs(jogVelocityCPS)); motorStopped = false; increasingBuoyancy = true; decreasingBuoyancy = false; return (true); } else { EngrLogger.writeToColumns("ERROR: Elmo.increasingBuoyancy: Bellows at lower limit"); return (false); } } public bool decreasingBuoyancy = false; public bool decreaseBuoyancy(int jogVelocityCPS) { if (!SV.BellowsAtUpperLimit) { if (!isValveOpen()) { openMV(); Thread.Sleep(1500); } pumpMotor.sendJV((-1 * System.Math.Abs(jogVelocityCPS))); motorStopped = false; decreasingBuoyancy = true; increasingBuoyancy = false; return (true); } else { EngrLogger.writeToColumns("ERROR: Elmo.decreaseBuoyancy: Bellows at uppper limit"); return (false); } } public bool isValveOpen() { return (!mb.getMVOpenedStatus()); } public static bool valveOpen; public void openMV() { EngrLogger.writeToColumns("Starting valve open"); mb.openMV(); valveOpen = true; } public void closeMV() { EngrLogger.writeToColumns("Starting valve close"); mb.closeMV(); valveOpen = false; } public int stopMotorAndCloseValve() { stopMotor(); Thread.Sleep(500); closeMV(); return (0); } public int getMotorStatus() { pumpMotor.sendSR(); return (1); } // Below is legacy CPF code public struct PlatVel { //PID constants //The values for K and Td have been updated to reflect the new .156 cc/rev pump //The old values K = 80 and Td = 10 were for the .692 cc/rev pump public const double K = 4.4 * 80.0, // Constant for P term of PID Ti = 50000.0, // Time constant for I term of PID (in denominator, bigger values less I term in output Td = 4.4 * 10.0, // Time constant for D term of PID Tf = 10.0, // Derivative filter time constant Tt = 100.0, // Anti-windup time constant B = 1.0, // Setpoint weight uLow = -2500.0 / 60.0, // minimum possible output (RPS) uHigh = 2500.0 / 60.0; // maximum possible output (RPS) public static double h = missionConfig.SBE41SamplePeriod / 1000.0; // Sample period (seconds) //Precomputed constants public static double den = (Tf * Tf) + (2 * h * Tf) + (2 * h * h), // denominator of measured signal filter terms p1 = Tf * Tf / den, // first part of state variable x2 equation p2 = 2 * h * h / den, // second part of state variable x1 equation p3 = K * h / Ti, // integral gain p4 = K * Td / h, // derivative gain p5 = h / Tt; // anti-windup gain //Loop variables public static double I = 0, // Integrator part of PID calculation y1 = 0, // first state variable y2 = 0, // second state variable v = 0.0, // unclipped PID output u = 0.0; // clipped PID output public static int uCPS = 0; // u value in counts per second } public struct PIDLogValues { public double setPoint; public double PV; public double y1; public double y2; public double I; public double v; public double u; public double uCPS; public double h; } private static PIDLogValues BCPIDLogValues; public void resetPVPID() { PlatVel.I = 0; PlatVel.y1 = 0; PlatVel.y2 = 0; PlatVel.v = 0.0; PlatVel.u = 0.0; PlatVel.uCPS = 0; } public void calcPIDConstants() { PlatVel.h = missionConfig.SBE41SamplePeriod; PlatVel.den = (PlatVel.Tf * PlatVel.Tf) + (2 * PlatVel.h * PlatVel.Tf) + (2 * PlatVel.h * PlatVel.h); // denominator of measured signal filter terms PlatVel.p1 = PlatVel.Tf * PlatVel.Tf / PlatVel.den; // first part of state variable x2 equation PlatVel.p2 = 2 * PlatVel.h * PlatVel.h / PlatVel.den; // second part of state variable x1 equation PlatVel.p3 = PlatVel.K * PlatVel.h / PlatVel.Ti; // integral gain PlatVel.p4 = PlatVel.K * PlatVel.Td / PlatVel.h; // derivative gain PlatVel.p5 = PlatVel.h / PlatVel.Tt; // anti-windup gain resetPVPID(); sbTemp.Clear(); sbTemp.Append("Calculated new PID Constant, h = "); sbTemp.Append(PlatVel.h.ToString()); EngrLogger.writeToColumns(sbTemp); } //Pump motor encoder counts per revolution public const double COUNTSperREV = 2048 * 4.0; //2048 Counts per Revolution Encoder *4 for quadrature interpolation public PIDLogValues PIDPlatformVelocity(double velocitySetPoint, double currentPlatVel) { PlatVel.h = SV.CtdSamplePeriod; //Astrom & Hagglund PID Algorithm PlatVel.y2 = (PlatVel.p1 * PlatVel.y2) + (PlatVel.p2 * (currentPlatVel - PlatVel.y1)); // update filter state y2 PlatVel.y1 = PlatVel.y1 + PlatVel.y2; // update filter state y1 PlatVel.v = PlatVel.K * ((PlatVel.B * velocitySetPoint) - PlatVel.y1) - // compute nominal output (PlatVel.p4 * PlatVel.y2) + PlatVel.I; // limit the nominal output to desired range if (PlatVel.v < PlatVel.uLow) PlatVel.u = PlatVel.uLow; else if (PlatVel.v > PlatVel.uHigh) PlatVel.u = PlatVel.uHigh; else PlatVel.u = PlatVel.v; // update the integral PlatVel.I = PlatVel.I + (PlatVel.p3 * (velocitySetPoint - PlatVel.y1)) + (PlatVel.p5 * (PlatVel.u - PlatVel.v)); //Convert Rev per second to Counts per second PlatVel.uCPS = (int)(PlatVel.u * COUNTSperREV); if (PlatVel.uCPS >= 0) { increaseBuoyancy(PlatVel.uCPS); } else { decreaseBuoyancy(PlatVel.uCPS); } BCPIDLogValues.I = PlatVel.I; BCPIDLogValues.PV = currentPlatVel; BCPIDLogValues.setPoint = velocitySetPoint; BCPIDLogValues.u = PlatVel.u; BCPIDLogValues.uCPS = PlatVel.uCPS; BCPIDLogValues.v = PlatVel.v; BCPIDLogValues.y1 = PlatVel.y1; BCPIDLogValues.y2 = PlatVel.y2; BCPIDLogValues.h = PlatVel.h; return (BCPIDLogValues); } public double DualLeadDepthController(double depthSetPoint, double currentPlatDepth) { // AUTHOR: Brian Ha (2018 Summer Intern) // MENTOR: Gene Massion // This function serves as a discrete implementation of a dual lead controller designed in MATLAB. // It assumes that depthSetPoint and currentPlatDepth are both measured in meters. // TO-FIX: Variables need to persist between function calls // TO-FIX: prev and prevPrev variables need to be initialized to zero. // TO-FIX: Check units of inputs. (are they in meters or pressure units?) // TO-FIX: Check units of output (does revs/sec work or should it be in units of counts?) // TO-FIX: Reference governor? // TO-FIX: Impose actuator limit? double nextMotorSpeedCommand; double prevMotorSpeedCommand; double prevPrevMotorSpeedCommand; double currentDepthError; double prevDepthError; double prevPrevDepthError; currentDepthError = depthSetPoint - currentPlatDepth; nextMotorSpeedCommand = 29.07*currentDepthError - 56.78*prevDepthError + 27.71*prevPrevDepthError + 1.238*prevMotorSpeedCommand - 0.3829*prevPrevMotorSpeedCommand; // Enforce motor speed limit [revs/sec] if (nextMotorSpeedCommand > 50.0) { nextMotorSpeedCommand = 50.0; } // Set values for next function call prevPrevMotorSpeedCommand = prevMotorSpeedCommand; prevPrevDepthError = prevDepthError; prevMotorSpeedCommand = nextMotorSpeedCommand; prevDepthError = currentDepthError; // Return motor speed command [revs/sec]. return (nextMotorSpeedCommand); } } }