public FieldbusInputData()
{
Control_I1 = new ControlWordI1();
BinaryOut_I2 = new BinaryOutputs_I2();
Control_I3 = new ControlWordI3();
SetpointVelocity = new SetpointVelocity();
Acceleration = new Acceleration();
Deceleration = new Deceleration();
Setpoint_Position = new SetpointPosition();
Subcontrol = new SubcontrolWord();
BinaryOut_I10 = new BinaryOutputsI10();
SetpointValue = new SetpointValue2();
}
public byte[] testing(byte[] array)
{
int accel = 0;
int rvel = 0;
int time = 0;
int decel = 0;
bool rpause = false;
String size = "12";
int startAddress = 4;
//int acceleration = 0;
SetpointVelocity setPointVelocity = new SetpointVelocity();
int acceleration = Convert.ToInt16(accel);
float f;
int runTime = time;
stopwatch.Reset();
//System.Timers.Timer timer = new System.Timers.Timer(runTime);
ushort ID = 8;
byte unit = 0;
ushort StartAddress = Convert.ToUInt16(startAddress); //ReadStartAdr();
data = GetDataNew(Convert.ToByte(size));
byte[] bytes = BitConverter.GetBytes(setPointVelocity.GetSetValue);
Console.WriteLine(bytes + " is set point velo");
Console.WriteLine(rvel);
byte[] bytes2 = BitConverter.GetBytes(acceleration);
ControlWord_I3 wordi3 = new ControlWord_I3();
data[0] = 0; //byte 0 of Control 1
data[1] = 0; //byte 1 of Control 1
data[2] = 0; //byte 0 of Binary Outputs
data[3] = 0; //byte 1 of Binary Outputs
data[4] = 10; //byte 0 of Control 3
data[5] = 6; //byte 1 of Control 3
// data[6] = BitConverter.GetBytes(setPointVelocity.GetSetValue)[1]; // byte 1 of Velocity
data[6] = BitConverter.GetBytes(rvel)[1];
//data[7] = BitConverter.GetBytes(setPointVelocity.GetSetValue)[0]; // byte 0 of velocity
data[7] = BitConverter.GetBytes(rvel)[0];
data[8] = BitConverter.GetBytes(acceleration)[1]; // byte 1 of Velocity
data[9] = BitConverter.GetBytes(acceleration)[0]; // byte 0 of velocity
data[10] = BitConverter.GetBytes(decel)[1];
data[11] = BitConverter.GetBytes(decel)[0];
array = data;
return(array);
}
//DoWork Method runs the motor for testing
public void DoWork()
{
foreach (Cue currentcue in SelectedQueues.Items)
{
Cue tmp = currentcue;
List <Triggers> trigs = currentcue.ReturnTrigger();
Console.WriteLine(trigs.Count());
foreach (Triggers trig in trigs.ToList())
{
log.AddRow("New trigger started");
int accel = trig.Acceleration();
int rvel = trig.Velocity();
int time = trig.Time();
int decel = trig.Deceleration();
bool rpause = trig.Pause();
String dir = trig.Direction();
int rot = trig.Rotation();
log.AddRow("Running this trigger" + " Pause: " + trig.Pause() + "\nRotation: " + rot + " " + dir + "\nAcceleration: " + accel + " m/s^2\n Deceleration:" + decel + "m/s^2\n Velocity: " + rvel + " m/s\n Time: " + time + " sec\n on motor" + trig.Motor());
String size = "12";
int startAddress = 4;
//int acceleration = 0;
SetpointVelocity setPointVelocity = new SetpointVelocity();
int acceleration = Convert.ToInt16(accel);
float f;
int runTime = time;
stopwatch.Reset();
//System.Timers.Timer timer = new System.Timers.Timer(runTime);
ushort ID = 8;
byte unit = 0;
ushort StartAddress = Convert.ToUInt16(startAddress); //ReadStartAdr();
data = GetDataNew(Convert.ToByte(size));
byte[] bytes = BitConverter.GetBytes(setPointVelocity.GetSetValue);
Console.WriteLine(bytes + " is set point velo");
Console.WriteLine(rvel);
byte[] bytes2 = BitConverter.GetBytes(acceleration);
ControlWord_I3 wordi3 = new ControlWord_I3();
this.Invoke((MethodInvoker) delegate()
{
if (dir.Equals("Clockwise"))
{
wordi3.Positive = false;
wordi3.Negative = true;
}
else
{
wordi3.Positive = true;
wordi3.Negative = false;
}
});
data[0] = 0; //byte 0 of Control 1
data[1] = 0; //byte 1 of Control 1
data[2] = 0; //byte 0 of Binary Outputs
data[3] = 0; //byte 1 of Binary Outputs
data[4] = BitConverter.GetBytes(wordi3.GetSetValue)[1]; //byte 0 of Control 3
data[5] = BitConverter.GetBytes(wordi3.GetSetValue)[0]; //byte 1 of Control 3 // data[6] = BitConverter.GetBytes(setPointVelocity.GetSetValue)[1]; // byte 1 of Velocity
data[6] = BitConverter.GetBytes(rvel)[1]; // byte 1 of velocity
data[7] = BitConverter.GetBytes(rvel)[0]; // byte 0 of velocity
data[8] = BitConverter.GetBytes(acceleration)[1]; // byte 1 of Velocity
data[9] = BitConverter.GetBytes(acceleration)[0]; // byte 0 of velocity
data[10] = BitConverter.GetBytes(decel)[1];
data[11] = BitConverter.GetBytes(decel)[0];
MBmaster.ReadWriteMultipleRegister(ID, unit, StartAddress, 12, StartAddress, data, ref result);
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
if (trig.Rotation() == 0) //Trigger by Time
{
if (trig.Deceleration() != 0)
{
runTime = trig.Time() + ((previous - trig.Velocity()) / trig.Deceleration());
}
stopwatch.Start();
Console.WriteLine("Running Trigger");
while (stopwatch.Elapsed < TimeSpan.FromSeconds(runTime))
{
//Console.WriteLine(howLong + " " + endTime);
if (!rpause)
{
MBmaster.ReadWriteMultipleRegister(ID, unit, StartAddress, 12, StartAddress, data, ref result);
}
//Console.WriteLine("working " + stopwatch.Elapsed.ToString());
// }
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
int velocity = result[6] * 256 + result[7];
this.Invoke((MethodInvoker) delegate()
{
textBox1.Text = velocity.ToString();;
textBox2.Text = dir;
textBox3.Text = textBox3.Text = Program.playView.position.Degrees().ToString();
});
}
stopwatch.Stop();
}
else //Trigger by rotation
{
Console.WriteLine("rotation != 0 " + trig.Rotation());
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
if (trig.Direction().Equals("Clockwise"))
{
int degrees = trig.Rotation();
int positions = degrees * 2100;
// rotation = Convert.ToInt16(textBoxCW.Text) - Program.playView.position.turnedDegrees;
//cw
int startingPos = Program.playView.position.currentPosition;
Console.WriteLine("Starting pos: " + startingPos + "Ending Position: " + (startingPos - positions));
while (Program.playView.position.currentPosition > (startingPos - positions))
{
int velocity = result[6] * 256 + result[7];
MBmaster.ReadWriteMultipleRegister(ID, unit, StartAddress, 12, StartAddress, data, ref result);
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
this.Invoke((MethodInvoker) delegate()
{
textBox1.Text = velocity.ToString();;
textBox2.Text = dir;
textBox3.Text = Program.playView.position.Degrees().ToString();
});
}
}
else//ccw
{
int degrees = trig.Rotation();
int positions = degrees * 2100;
int startingPos = Program.playView.position.currentPosition;
while (Program.playView.position.currentPosition < (startingPos + positions))
{
int velocity = result[6] * 256 + result[7];
MBmaster.ReadWriteMultipleRegister(ID, unit, StartAddress, 12, StartAddress, data, ref result);
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
this.Invoke((MethodInvoker) delegate()
{
textBox3.Text = Program.playView.position.Degrees().ToString();
textBox1.Text = velocity.ToString();;
textBox2.Text = dir;
});
}
}
previous = trig.Velocity();
}
}
}
this.Invoke((MethodInvoker) delegate()
{
button1.Enabled = true;
button2.Enabled = true;
button3.Enabled = true;
button5.Enabled = true;
button6.Enabled = true;
button7.Enabled = true;
button8.Enabled = true;
log.AddRow("Queue Finished");
button4.Enabled = false;
button6.Text = "Play/Pause";
});
}
//DoWork Method runs the motor for testing
public void DoWork()
{
String size = "12";
int startAddress = 4;
SetpointVelocity setPointVelocity = new SetpointVelocity();
int acceleration = Convert.ToInt16(textBoxAccel.Text);
this.Invoke((MethodInvoker) delegate()
{
String direction = comboBoxCW.SelectedItem.ToString();
});
int rotation = Convert.ToInt16(textBoxCW.Text);
float f;
int runTime = Convert.ToInt16(textBoxTime.Text);
stopwatch.Reset();
if ((string.IsNullOrWhiteSpace(textBoxAccel.Text) || !float.TryParse(textBoxAccel.Text, out f) || Convert.ToInt16(textBoxAccel.Text) == 0 || acceleration == 0))
{
MessageBox.Show("Must input a non-zero decimal value for Acceleration!");
}
if (string.IsNullOrWhiteSpace(textBoxVel.Text) || !float.TryParse(textBoxVel.Text, out f))
{
MessageBox.Show("Must input a decimal value for velocity!");
}
else if (acceleration != 0)
{
setPointVelocity.GetSetValue = Convert.ToInt16(textBoxVel.Text);
}
ushort ID = 8;
byte unit = 0;
ushort StartAddress = Convert.ToUInt16(startAddress); //ReadStartAdr();
data = GetDataNew(Convert.ToByte(size));
/*byte[] bytes = BitConverter.GetBytes(setPointVelocity.GetSetValue);
* byte[] bytes2 = BitConverter.GetBytes(acceleration);*/
ControlWord_I3 wordi3 = new ControlWord_I3();
if (direction.Equals("Clockwise"))
{
wordi3.Positive = false;
wordi3.Negative = true;
}
else
{
wordi3.Positive = true;
wordi3.Negative = false;
}
data[0] = 0; //byte 0 of Control 1
data[1] = 0; //byte 1 of Control 1
data[2] = 0; //byte 0 of Binary Outputs
data[3] = 0; //byte 1 of Binary Outputs
data[4] = BitConverter.GetBytes(wordi3.GetSetValue)[1]; //byte 0 of Control 3
data[5] = BitConverter.GetBytes(wordi3.GetSetValue)[0]; //byte 1 of Control 3
data[6] = BitConverter.GetBytes(setPointVelocity.GetSetValue)[1]; // byte 1 of Velocity
data[7] = BitConverter.GetBytes(setPointVelocity.GetSetValue)[0]; // byte 0 of velocity
data[8] = BitConverter.GetBytes(acceleration)[1]; // byte 1 of Velocity
data[9] = BitConverter.GetBytes(acceleration)[0]; // byte 0 of velocity
data[10] = BitConverter.GetBytes(deceleration)[1];
data[11] = BitConverter.GetBytes(deceleration)[0];
MBmaster.ReadWriteMultipleRegister(8, 0, Convert.ToUInt16(4), 12, Convert.ToUInt16(4), data, ref result);
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
float positionBefore = Program.playView.position.currentPosition;
long position = Program.playView.position.currentPosition;
Console.WriteLine("current pos " + Program.playView.position.currentPosition);
if (rotation == 0)
{
Console.WriteLine("rotation = 0");
if (textBoxAccelDecelTime.Text != "")
{
if (deceleration == 0)
{
int time = Convert.ToInt16(textBoxAccelDecelTime.Text);
}
else
{
int decelTime = Math.Abs(previous - velocity) / deceleration;
time = decelTime;
}
stopwatch.Start();
while (stopwatch.Elapsed < TimeSpan.FromSeconds(runTime + time))
{
MBmaster.ReadWriteMultipleRegister(ID, unit, StartAddress, 12, StartAddress, data, ref result);
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
this.Invoke((MethodInvoker) delegate()
{
textBox3.Text = Program.playView.position.Degrees().ToString();
textBox4.Text = Program.playView.position.currentPosition.ToString();
});
}
stopwatch.Reset();
}
else
{
stopwatch.Start();
while (stopwatch.Elapsed < TimeSpan.FromSeconds(runTime))
{
this.Invoke((MethodInvoker) delegate()
{
textBox3.Text = Program.playView.position.Degrees().ToString();
});
this.Invoke((MethodInvoker) delegate()
{
textBox4.Text = Program.playView.position.currentPosition.ToString();
});
}
int timeAfter = DateTime.Now.Second;
Console.WriteLine("time after " + timeAfter);
Console.WriteLine("finalpos " + position);
Console.WriteLine("Rate of position over time " + (position - positionBefore) / runTime);
}
}
else //Movement by Rotation
{
MBmaster.ReadWriteMultipleRegister(8, 0, Convert.ToUInt16(4), 12, Convert.ToUInt16(4), data, ref result);
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
Console.WriteLine("rotation != 0 " + rotation);
if (direction.Equals("Clockwise"))
{
int degrees = Convert.ToInt16(textBoxCW.Text);
int positions = degrees * 2100;
// rotation = Convert.ToInt16(textBoxCW.Text) - Program.playView.position.turnedDegrees;
//cw
int startingPos = Program.playView.position.currentPosition;
Console.WriteLine("Starting pos: " + startingPos + "Ending Position: " + (startingPos - positions));
while (Program.playView.position.currentPosition > (startingPos - positions))
{
MBmaster.ReadWriteMultipleRegister(ID, unit, StartAddress, 12, StartAddress, data, ref result);
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
this.Invoke((MethodInvoker) delegate()
{
textBox3.Text = Program.playView.position.Degrees().ToString();
textBox4.Text = Program.playView.position.currentPosition.ToString();
});
}
}
else//ccw
{
int degrees = Convert.ToInt16(textBoxCW.Text);
int positions = degrees * 2100;
int startingPos = Program.playView.position.currentPosition;
while (Program.playView.position.currentPosition < (startingPos + positions))
{
MBmaster.ReadWriteMultipleRegister(ID, unit, StartAddress, 12, StartAddress, data, ref result);
Program.playView.position.currentPosition = result[12] * 256 * 256 * 256 + result[13] * 256 * 256 + result[14] * 256 + result[15];
this.Invoke((MethodInvoker) delegate()
{
textBox3.Text = Program.playView.position.Degrees().ToString();
textBox4.Text = Program.playView.position.currentPosition.ToString();
});
}
}
}
previous = velocity;
Console.WriteLine("previous " + previous);
}