using System; using System.Threading; using System.IO.Ports; using System.Text; using Microsoft.SPOT; using Microsoft.SPOT.Hardware; using GHI.Premium.Hardware; namespace MFConsoleApplication1 { public class Program { public static void Main() { Debug.Print("\r\nStarting Program ... \r\n"); int loopNum = 0; //DGH stuff int DGHReadCount = 0; int DGHBytesRead = 0; byte[] DGHTxBytes; byte[] DGHRxBytes = new byte[2]; byte[] DGHRxByteArray = new byte[20]; byte[] strippedDGHRxByteArray = new byte[15]; char[] DGHRxCharArray = new char[20]; string DGHRead = "$1RD\r"; //$1RD terminated with CR is the command to read data from the DGH string DGHRxString = ""; double pressureDBar = 1000.0; double ma2bar = (15.0/1.45038) / 16.0; //4-20 ma transducer; 15 psig full scale; 1 dBar = 1.45038 psi double maOffset = 4.0; //Elmo Stuff byte[] ElmoTxBytes; string ElmoStop = "ST;"; string ElmoMotorOn = "MO=1;"; string ElmoMotorOff = "MO=0;"; string ElmoJogVelocity = "JV="; string ElmoBegin = "BG;"; string ElmoVelocityMode = "UM=2;"; string ElmoJVString = "0.0"; //Console port sutff int consoleBytesRead = 0; int consoleReadCount = 0; bool waitForBG = true; byte[] consoleTxBytes; byte[] consoleRxBytes = new byte[2]; byte[] consoleRxByteArray = new byte[3]; char[] consoleRxCharArray = new char[3]; string consoleBegin = "BG"; //string consoleStop = "ST"; string consoleUpload = "UP"; string consoleRxString = ""; double RPM2CPS = 500.0 / 60.0; //500 Counts per Rev divided by 60 seconds per minu //PID algorithm parameters double K = 100.0, // Constant for P term of PID Ti = 5000.0, // Time constant for I term of PID Td = 0.1, // Time constant for D term of PID Tf = 2.0, // Derivative filter time constant Tt = 1.0, // Anti-windup time constant B = 1.0, // Setpoint weight h = 0.1, // Sample period (seconds) 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 uCPS = 0.0, // u value in counts per second uLow = -3000.0, // minimum possible output uHigh = 3000; // maximum possible output // Pre-computed constants for PID algorithm 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 DateTime loopTime; DateTime atDepthStartTime = new DateTime(2010, 1, 1, 1, 0, 0); TimeSpan cycleTimeDiff; TimeSpan atDepthTimeDiff; int atDepthDwellTime = 3; //Minutes spent at depth before surfacing bool atDepthTimeSet = false; bool runProfile = false; // Mission parameters double pressureSP = 5.0; // dBar double setPoint = 0.0; Debug.Print("Setting the RTC to 2013Jan01 00:00:00"); DateTime timeNow; //String for logging data int maxLogLength = 5000; string[] dataLogString = new string[maxLogLength]; string dataLogStringHeader = ""; //Setup the DGH RS232 port for the pressure transducer SerialPort DGHPort = new SerialPort("COM1", 38400, Parity.None, 8, StopBits.One); DGHPort.ReadTimeout = 0; DGHPort.Open(); DGHPort.Flush(); DGHTxBytes = Encoding.UTF8.GetBytes(DGHRead); //Setup the Elmo RS232 port for motor control SerialPort ElmoPort = new SerialPort("COM2", 19200, Parity.None, 8, StopBits.One); ElmoPort.ReadTimeout = 0; ElmoPort.Open(); ElmoPort.Flush(); Debug.Print("Stopping the BE Motor"); //Setup the Elmo //Stop the Elmo ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoStop); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); Thread.Sleep(1); //Elmo Motor off ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoMotorOff); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); Thread.Sleep(1); //Set velocity mode ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoVelocityMode); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); Thread.Sleep(1); //Set jog velocity to 0 ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoJogVelocity + "0;"); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); Thread.Sleep(1); //Turn Motor on ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoMotorOn); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); Thread.Sleep(1); //Begin motion ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoBegin); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); Thread.Sleep(1); //Setup the Console port for commands in and data out SerialPort ConsolePort = new SerialPort("COM3", 19200, Parity.None, 8, StopBits.One); ConsolePort.ReadTimeout = 0; ConsolePort.Open(); ConsolePort.Flush(); while (true) { //Write welcome string consoleTxBytes = Encoding.UTF8.GetBytes("\r\nBG to start; UP to upload data\r\n"); ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); Thread.Sleep(1); Debug.Print("Waiting for BG (or bg) command"); //wait for console begin command while (waitForBG) { while (consoleBytesRead < 2) { consoleReadCount = ConsolePort.Read(consoleRxBytes, 0, 1); if (consoleReadCount > 0) { consoleRxByteArray[consoleBytesRead] = consoleRxBytes[0]; consoleBytesRead = consoleBytesRead + 1; } else Thread.Sleep(1); } consoleRxCharArray = Encoding.UTF8.GetChars(consoleRxByteArray); consoleRxString = new string(consoleRxCharArray); if (consoleRxString.ToUpper() == consoleBegin) { waitForBG = false; setPoint = pressureSP; runProfile = true; Array.Clear(dataLogString, 0, dataLogString.Length); } else if (consoleRxString.ToUpper() == consoleUpload) { if (dataLogString.Length > 0) { consoleTxBytes = Encoding.UTF8.GetBytes(dataLogStringHeader); ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); Thread.Sleep(1); for (int i = 0; i < dataLogString.Length; i++) { consoleTxBytes = Encoding.UTF8.GetBytes(dataLogString[i]); ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); Thread.Sleep(1); } } else { consoleTxBytes = Encoding.UTF8.GetBytes("No data yet"); ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); Thread.Sleep(1); } } else { consoleReadCount = 0; consoleBytesRead = 0; } } Debug.Print("Waiting 5 minutes to get CPF in the tank"); /**********Insert 5 minute wait here ********/ consoleTxBytes = Encoding.UTF8.GetBytes("Starting BE PID loop\r\n"); ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); Debug.Print("Starting the BE PID loop"); //Start the depth control loop while (runProfile) { loopTime = DateTime.Now; //Send the DGH read command DGHPort.Write(DGHTxBytes, 0, DGHTxBytes.Length); Debug.Print("Waiting for DGH pressure value"); // read the data, should terminated with a CR DGHBytesRead = 0; while (DGHBytesRead < 12) { DGHReadCount = DGHPort.Read(DGHRxBytes, 0, 1); if (DGHReadCount > 0) { DGHRxByteArray[DGHBytesRead] = DGHRxBytes[0]; DGHBytesRead = DGHBytesRead + 1; } else Thread.Sleep(1); } //Get rid of non-characters in DGH byte array for (int i = 2; i < 11; i++) { strippedDGHRxByteArray[i - 2] = DGHRxByteArray[i]; } //Convert to bar (pressure) DGHRxCharArray = Encoding.UTF8.GetChars(strippedDGHRxByteArray); DGHRxString = new string(DGHRxCharArray); pressureDBar = Convert.ToDouble(DGHRxString); pressureDBar = (pressureDBar - maOffset) * ma2bar; //PID Algorithm y2 = (p1 * y2) + (p2 * (pressureDBar - y1)); // update filter state x2 y1 = y1 + y2; // update filter state x1 v = K * ((B * setPoint) - y1) - (p4 * y2) + I; // compute nominal output if (v < uLow) // limit the nominal output to desired range u = uLow; else if (v > uHigh) u = uHigh; else u = v; I = I + (p3 * (setPoint - y1)) + (p5 * (u - v)); // update the integral Debug.Print("Sending new speed to Elmo"); //Send the new velocity to the Elmo uCPS = u * RPM2CPS; ElmoJVString = ElmoJogVelocity + uCPS.ToString("F0") + ";"; ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoJVString); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoBegin); //ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); //Check to see if we're at depth. If true, set pressure set point to 0 (return to surface) if (atDepthTimeSet == false) { if ((pressureDBar > (setPoint * 1.05)) || (pressureDBar < pressureSP * 0.95)) { atDepthStartTime = DateTime.Now; atDepthTimeSet = true; } } else { timeNow = DateTime.Now; atDepthTimeDiff = timeNow.Subtract(atDepthStartTime); if (atDepthTimeDiff.Minutes >= atDepthDwellTime) { pressureSP = 0.0; atDepthTimeSet = false; } } dataLogStringHeader = "\r\nTime,Pressure,y1,y2,v,u,I\r\n"; if( dataLogString.Length < maxLogLength ) dataLogString[loopNum] = loopTime.Date.ToString() + "." + loopTime.Millisecond.ToString("I3") + "," + pressureDBar.ToString("F3") + "," + y1.ToString("F3") + "," + y2.ToString("F3") + "," + v.ToString("F3") + "," + u.ToString("F3") + "," + I.ToString("F3"); /*pressureDBarLog[loopNum] = pressureDBar; y2Log[loopNum] = y2; y1Log[loopNum] = y1; vLog[loopNum] = v; uLog[loopNum] = u; ILog[loopNum] = I; loopTimeLog[loopNum] = loopTime.Date.ToString() + loopTime.Millisecond.ToString(); */ string tempStr; tempStr = "Loop time = " + loopTime.ToString() + "." + loopTime.Millisecond.ToString(); Debug.Print(tempStr); consoleTxBytes = Encoding.UTF8.GetBytes(tempStr); //ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); tempStr = "Loop Number = " + loopNum.ToString(); Debug.Print(tempStr); consoleTxBytes = Encoding.UTF8.GetBytes(tempStr); //ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); tempStr = "Pressure (dBar)= " + pressureDBar.ToString(); Debug.Print(tempStr); consoleTxBytes = Encoding.UTF8.GetBytes(tempStr); //ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); tempStr = "Y2 = " + y2.ToString(); Debug.Print(tempStr); consoleTxBytes = Encoding.UTF8.GetBytes(tempStr); //ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); tempStr = "Y1 = " + y1.ToString(); Debug.Print(tempStr); consoleTxBytes = Encoding.UTF8.GetBytes(tempStr); //ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); tempStr = "v (RPM) = " + v.ToString(); Debug.Print(tempStr); consoleTxBytes = Encoding.UTF8.GetBytes(tempStr); //ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); tempStr = "u (RPM) = " + u.ToString(); Debug.Print(tempStr); consoleTxBytes = Encoding.UTF8.GetBytes(tempStr); //ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); tempStr = "u (counts per second) = " + uCPS.ToString(); Debug.Print(tempStr); consoleTxBytes = Encoding.UTF8.GetBytes(tempStr); //ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); tempStr = "I = " + I.ToString() + "\r\n"; Debug.Print(tempStr); consoleTxBytes = Encoding.UTF8.GetBytes(tempStr); //ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length); //Wait for next cycle time do { Debug.Print("Waiting for end of 0.1 second cycle"); timeNow = DateTime.Now; cycleTimeDiff = timeNow.Subtract(loopTime); /***** Make sure to change this back to 0.1 ************/ } while (cycleTimeDiff.Seconds < 2); loopNum++; } //Stop the Elmo ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoStop); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); Thread.Sleep(1); //Elmo Motor off ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoMotorOff); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); Thread.Sleep(1); //Set jog velocity to 0 ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoJogVelocity + "0;"); ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length); Thread.Sleep(1); } } } }