private void ResetSaftey(object sender, RoutedEventArgs e)
{
t.Enabled = false;
ADSQL.SqlWriteAxis(1, "AxisStatus", "RESET");
VirtualModeButton.Opacity = 20;
SafteyResetButton.Visibility = Visibility.Hidden;
ModeLabel.Content = "Resetting Saftey System...";
LiveDisplay = 1;
Thread.Sleep(500);
t.Enabled = true;
ModeSelect();
}
private void AxisControlTextBox_LostKeyboardFocus(object sender, RoutedEventArgs e)
{
this.HasKeyBoardFocus = false;
System.Windows.Controls.TextBox t = (System.Windows.Controls.TextBox)sender;
if (SelectedQue != "" && SelectedQue != null)
{
ADSQL.SqlWriteAxis(this.AxisNumber, t.Tag.ToString(), t.Text, false, SelectedQue);
}
else
{
ADSQL.SqlWriteAxis(this.AxisNumber, t.Tag.ToString(), t.Text);
}
}
private void AxisTextbox_KeyDown(object sender, KeyEventArgs e)
{
System.Windows.Controls.TextBox t = (System.Windows.Controls.TextBox)sender;
if (e.Key == Key.Enter)
{
if (SelectedQue != "" && SelectedQue != null)
{
ADSQL.SqlWriteAxis(this.AxisNumber, t.Tag.ToString(), t.Text, false, SelectedQue);
}
else
{
ADSQL.SqlWriteAxis(this.AxisNumber, t.Tag.ToString(), t.Text);
}
this.RemoveButton.Focus();
}
}
void ADSQLScan(ADSClient Mom)
{
do
{
Thread.Sleep(10);
if (ReadTimer.ElapsedMilliseconds > ReadInterval)
{
ReadTimer = Stopwatch.StartNew();
ReadTimer.Stop();
currentValue = ADSQL.SqlReadAxis(AxisNum, ColumnName);
if (currentValue != WriteValue && WriteValue != null)
{
WriteKidValue(AxisNum, ColumnName, WriteValue, Mom);
}
ADSQL.SqlWriteAxis(AxisNum, ColumnName, ReadKidValue(AxisNum, ColumnName, Mom, false));
ReadTimer = Stopwatch.StartNew();
}
} while (Running);
}
public AxisParam(int axisNum, string columnName, int readInterval, int writeInterval, ADSClient Mom, Object InitValue = null)
{
AxisNum = axisNum;
ReadInterval = readInterval;
ColumnName = columnName;
ReadTimer = Stopwatch.StartNew();
if (writeInterval != 0)
{
WriteTimer = Stopwatch.StartNew();
}
// ScanTask = new Task(() => ADSQLScan(Mom));
if (InitValue == null)
{
ADSQL.SqlWriteAxis(AxisNum, ColumnName, ReadKidValue(AxisNum, ColumnName, Mom, false));
currentValue = ReadKidValue(axisNum, columnName, Mom);
}
//ADSQLScan();
// ScanTask.Start();
}
private void T_tick(object myObject, EventArgs e)
{
Stopwatch s = Stopwatch.StartNew();
try
{
ConnectSerial();
if (sr == null)
{
return;
}
t.Interval = 30;
if (disabled || t_Busy)
{
return;
}
t_Busy = true;
tickCounter++;
if (sr.EstopPressed && LiveDisplay != 2)
{
ADSQL.SqlWriteAxis(1, "AxisStatus", "FAULT"); ShadeRectangle1.Stroke = new SolidColorBrush(System.Windows.Media.Colors.Red);
}
// t.Stop();
// ConnectMom();
if (sr.DeadmanRightPressed || sr.DeadmanLeftPressed)
{
ADSQL.SqlWriteAxis(1, "LiveValues", true);
}
for (int x = 0; x < 6; x++)
{
KidPositions[x] = int.Parse(ADSQL.SqlReadAxis(x + 1, "CurrentPosition").ToString());
}
for (int x = 0; x < 4; x++)
{
if (qControl[x].IsActive)
{
sr.Lights[x] = 5;
}
else
{
sr.Lights[x] = 7;
}
if (sr != null)
{
if ((sr.DeadmanRightPressed || sr.DeadmanLeftPressed) && qControl[x].IsActive && sr.w[x] == 1)
{
if (qControl[x].IsActive)
{
sr.Lights[x] = 3;
}
ADSQL.ExecuteQue(qControl[x].QueName, Math.Abs(int.Parse(sr.y[x].ToString()) + 100)); //TextBlock1.Text = sr.y[x].ToString();
}
else if (sr != null)
{
if ((sr.DeadmanRightPressed || sr.DeadmanLeftPressed) && qControl[x].QueNameTextBox.Text.Contains("iBeamHoist"))
{
// if (sr.DeadmanRightPressed || sr.DeadmanLeftPressed) ADSQL.SqlWriteAxis(1, "isActive", true);
//TextBlock1.Text = "JS: " + JoySticks[x].Y.ToString() + " " + JoySticks[x].X.ToString() + " " + JoySticks[x].Z.ToString();
// Debug.Print("AXIS: " + qControl[x].AxisIndex.ToString());
//Debug.Print("JOYSTICK: " +(3 * JoySticks[x].Y).ToString());
// Debug.Print("POSITION: " +KidPositions[qControl[x].AxisIndex].ToString());
//Debug.Print("TARGET: " + int.Parse(Math.Round((5 * JoySticks[x].Y) + KidPositions[qControl[x].AxisIndex]).ToString()).ToString());
int p = int.Parse(sr.y[x].ToString()) * 5;
if (!ADSQL.ExecuteJog(qControl[x].QueName, Math.Abs(int.Parse(sr.y[x].ToString())), int.Parse(JogSpeedTextBox.Text), float.Parse(JogAccelTextBox.Text), p))
{
MessageBox.Show("EAT SHIT");
}
}
else if (sr != null)
{
if ((sr.DeadmanRightPressed || sr.DeadmanLeftPressed) && qControl[x].IsActive)
{
ADSQL.QueWatchDog(qControl[x].QueName, Math.Abs(int.Parse(sr.y[x].ToString()) + 100));
qControl[x].StatusTextBox.Text = (string)ADSQL.SqlReadQue(qControl[x].QueName, "QueStatus");
}
}
}
}
}
if ((bool)QueMode && qControl[loopCount % 4].IsActive)
{
qControl[loopCount % 4].StatusTextBox.Text = (string)ADSQL.SqlReadQue(qControl[loopCount % 4].QueName, "QueStatus");
}
UpdateCad(SelectedLayer);
for (int i = 0; i < JoySticks.Length; i++)
{
if (sp.IsOpen)
{
JoySticks[i].X = sr.x[i]; JoySticks[i].Y = sr.y[i]; JoySticks[i].Z = sr.z[i]; JoySticks[i].W = sr.w[i];
}
}
loopCount++;
loopCount = loopCount % 100;
if (loopCount % 2 == 1)
{
ModeSelect();
}
if (loopCount == 99)
{
QuesGrid_Update(QueGrid_Selection);
}
if (LiveDisplay != 2)
{
// if (Mom != null && sr != null && loopCount % 10 == 3) if (sr.DeadmanRightPressed || sr.DeadmanLeftPressed) Mom.WriteValue(Mom.DeadMan, true, "DEADMAN"); else if (loopCount % 10 == 3) Mom.WriteValue(Mom.DeadMan, false, "DEADMAN");
// if (Mom != null && sr != null) if (loopCount % 50 == 49 && Mom.Connected) Mom.WriteValue(Mom.MomControl, true, "MOMCONTROL");
}
t_Busy = false;
}
catch { }
finally { t_Busy = false; }
}
public static void Main(string[] args)
{
bool[] isActive = new bool[6];
int[] DeadCounter = new int[6];
try
{
int InstanceCount = Process.GetProcessesByName(Process.GetCurrentProcess().ProcessName).Length;
if (InstanceCount > 2)
{
return;
}
if (InstanceCount > 1)
{
do
{
InstanceCount = Process.GetProcessesByName(Process.GetCurrentProcess().ProcessName).Length; Thread.Sleep(200);
} while (InstanceCount > 1);
}
if (InstanceCount > 1)
{
Thread.Sleep(5000);
}
Console.Write("STARTING CONNECTION TO MOM...");
ADSClient Mom = new ADSClient(new TwinCAT.Ads.AmsAddress("10.99.1.1.1.1", 851).NetId.ToString(), true, 6);
Console.Write("DONE");
Stopwatch j = Stopwatch.StartNew();
int loopCount = 0;
do
{
loopCount++; Debug.Print(loopCount.ToString());
if (loopCount > 0)
{
Console.WriteLine(loopCount.ToString() + " " + j.ElapsedMilliseconds.ToString()); j = Stopwatch.StartNew();
}
Mom.WriteValue(Mom.TimeOutSwitch, false);
AxisRows = (DataRowCollection)ADSQL.SqlPullAxis();
Thread.Sleep(50);
bool GlobalReset = (string)ADSQL.SqlReadAxis(1, "AxisStatus") == "RESET";
if ((string)ADSQL.SqlReadAxis(1, "AxisStatus") == "FAULT")
{
Mom.WriteValue(Mom.GlobalEstop, true);
}
if (GlobalReset)
{
Mom.WriteValue(Mom.FaultReset, true);
}
bool GlobalEstop = (int)Mom.ReadValue(Mom.GlobalEstop) > 0;
for (int x = 1; x <= 6; x++)
{
if (GlobalReset)
{
ADSQL.SqlWriteAxis(x, "AxisStatus", "OK");
}
string FaultStatus = (string)ADSQL.SqlReadAxis(x, "AxisStatus");
if (loopCount > 200)
{
loopCount = 0;
}
bool QueStarted = (bool)ADSQL.SqlReadAxis(x, "QueStarted");
if (QueStarted)
{
Mom.Kids[x - 1].MomControl = 0; ADSQL.SqlWriteAxis(x, "QueStarted", false);
}
bool thisActive = (bool)ADSQL.SqlReadAxis(x, "IsActive");
int KidCurrent = Mom.Kids[x - 1].CurrentPosition;
ADSQL.SqlWriteAxis(x, "CurrentPosition", KidCurrent);
int KidTarget;
if (thisActive)
{
ADSQL.SqlWriteAxis(x, "IsActive", 0);
Mom.Kids[x - 1].DeadManPressed = 1;
KidTarget = Mom.Kids[x - 1].TargetPosition;
int KidSqlTarget = (int)ADSQL.SqlReadAxis(x, "TargetPosition");
int scaler = Math.Abs(KidCurrent - KidSqlTarget) / 5;
Debug.Print(scaler.ToString());
Mom.Kids[x - 1].ScalingInt = (int)ADSQL.SqlReadAxis(x, "TrimFactor");;
if (KidSqlTarget != KidTarget)
{
Mom.Kids[x - 1].TargetPosition = KidSqlTarget;
}
}
if (isActive[x - 1] == thisActive && !thisActive)
{
DeadCounter[x - 1]++;
if (DeadCounter[x - 1] > 3 && DeadCounter[x - 1] < 5)
{
Mom.Kids[x - 1].DeadManPressed = 0;
}
}
else
{
DeadCounter[x - 1] = 0;
}
int KidAccel = Mom.Kids[x - 1].ModeAccel;
int KidDecel = Mom.Kids[x - 1].ModeDecel;
int KidVel = Mom.Kids[x - 1].ModeVel;
int KidSqlDecel = (int)ADSQL.SqlReadAxis(x, "Deceleration");
int KidSqlAccel = (int)ADSQL.SqlReadAxis(x, "Acceleration");
int KidSqlVel = (int)ADSQL.SqlReadAxis(x, "Velocity");
bool JogMode = (bool)ADSQL.SqlReadAxis(x, "JogMode");
if (JogMode)
{
int accelRate = (int)((int)ADSQL.SqlReadAxis(x, "JogSpeed") / (double)ADSQL.SqlReadAxis(x, "JogAccel"));
KidSqlAccel = accelRate;
if (accelRate < 400)
{
accelRate = 400;
}
KidSqlDecel = accelRate;
KidSqlVel = (int)ADSQL.SqlReadAxis(x, "JogSpeed");
}
if (KidSqlAccel != KidAccel)
{
Mom.Kids[x - 1].ModeAccel = KidSqlAccel;
}
if (KidSqlDecel != KidDecel)
{
Mom.Kids[x - 1].ModeDecel = KidSqlDecel;
}
if (KidSqlVel != KidVel)
{
Mom.Kids[x - 1].ModeVel = KidSqlVel;
}
if (isActive[x - 1])
{
}
else
{
KidTarget = KidCurrent;
Mom.Kids[x - 1].TargetPosition = Mom.Kids[x - 1].CurrentPosition;
}
if (Mom.Kids[x - 1].MomControl == 0)
{
ADSQL.SqlWriteAxis(x, "AxisStatus", "OFFLINE");
}
else if (GlobalEstop && !GlobalReset)
{
ADSQL.SqlWriteAxis(x, "AxisStatus", "FAULT");
}
if (QueStarted)
{
Mom.Kids[x - 1].MomControl = 1;
}
isActive[x - 1] = thisActive;
}
} while (true);
}
catch
{
Thread.Sleep(20000); Console.WriteLine("Error");
Environment.Exit(0);
}
}