static void _motorConnectTimer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
{
string[] ports = SerialPort.GetPortNames();
bool _hemiMotorFound = false;
foreach (string port in ports)
{
try
{
if (_serialPort != null)
{
_serialPort.Close();
}
_serialPort = new SerialPort(port, 19200, Parity.None, 8, StopBits.One);
_serialPort.DtrEnable = true;
_serialPort.NewLine = "\r";
_serialPort.Encoding = System.Text.Encoding.ASCII;
_serialPort.ReadTimeout = 2000;
_serialPort.WriteTimeout = 2000;
if (!_serialPort.IsOpen)
{
_serialPort.Open();
}
_serialPort.DiscardInBuffer();
_serialPort.DiscardOutBuffer();
//write some nulls
_serialPort.Write("\0");
_serialPort.Write("\0");
_serialPort.Write("\0");
_serialPort.DiscardInBuffer();
_serialPort.DiscardOutBuffer();
_serialPort.WriteLine("MV");
string mvRead = _serialPort.ReadLine();
try
{
_model = (SERVO_MOTOR_MODEL)Convert.ToInt32(mvRead.Substring(4, 3));;
}
catch
{
Application.Current.Dispatcher.Invoke((Action)(() =>
App.LogEntry.AddEntry("Unsupported Hemisphere Motor Drive", true)));
}
Application.Current.Dispatcher.Invoke((Action)(() =>
App.LogEntry.AddEntry("Hemisphere Model/Revision: " + mvRead)));
_serialPort.WriteLine("OP");
Application.Current.Dispatcher.Invoke((Action)(() =>
App.LogEntry.AddEntry("Hemisphere Option Board: " + _serialPort.ReadLine())));
_serialPort.WriteLine("SC");
string response = _serialPort.ReadLine().Substring(3);
Application.Current.Dispatcher.Invoke((Action)(() =>
App.LogEntry.AddEntry("Hemisphere Status: 0x" + response)));
_serialPort.WriteLine("BR");
response = _serialPort.ReadLine().Substring(3);
if (response == "2")
{
_hemiMotorFound = true;
break;
}
}
catch
{
if (_serialPort != null)
{
_serialPort.Close();
}
}
}
if (!_hemiMotorFound)
{
if (_serialPort != null)
{
_serialPort.Close();
}
_motorConnectTimer.Start();
return;
}
_motorConnected = true;
Application.Current.Dispatcher.BeginInvoke(new Action(System.Windows.Input.CommandManager.InvalidateRequerySuggested));
string status = "";
_serialPort.DiscardInBuffer();
_serialPort.WriteLine("IS");
status = _serialPort.ReadLine();
var hState = 0;
if (status.Length == 11)
{
status = status.Substring(3);
hState = (Convert.ToByte(status.Substring(5, 3), 2));
}
_isOpen = (hState & 0x5) == 0x5;
_isClosed = (hState & 0x6) == 0x6;
_inputStatusPollTimer = new Timer();
_inputStatusPollTimer.Interval = 200;
_inputStatusPollTimer.AutoReset = false;
_inputStatusPollTimer.Elapsed += new ElapsedEventHandler(_inputStatusPollTimer_Elapsed);
_inputStatusPollTimer.Start();
//close on connect
Close(null, null);
}
public static bool Connect(string interfaceName, int numRetries = 1)
{
for (int i = 0; i < numRetries; i++)
{
try
{
App.LogEntry.AddEntry("Trying to connect to Stepper Motor");
if (_serialPort != null)
{
_serialPort.Close();
}
if (_statusTimer != null)
{
_statusTimer.Stop();
}
_serialPort = new System.IO.Ports.SerialPort(interfaceName, 9600, System.IO.Ports.Parity.None, 8, System.IO.Ports.StopBits.One);
_serialPort.DtrEnable = true;
_serialPort.NewLine = "\r";
_serialPort.Encoding = System.Text.Encoding.ASCII;
_serialPort.ReadTimeout = 2000;
_serialPort.WriteTimeout = 2000;
if (!_serialPort.IsOpen)
{
_serialPort.Open();
}
_serialPort.DiscardInBuffer();
_serialPort.DiscardOutBuffer();
//write some nulls
_serialPort.Write("\0");
_serialPort.Write("\0");
_serialPort.Write("\0");
_serialPort.DiscardInBuffer();
_serialPort.DiscardOutBuffer();
_serialPort.WriteLine("MV");
System.Threading.Thread.Sleep(100);
_serialPort.DiscardInBuffer();
//make sure in SCL mode
_serialPort.WriteLine("PM");
if (_serialPort.ReadLine().Substring(3) != "2")
{
_serialPort.WriteLine("PM2");
CheckAck();
App.LogEntry.AddEntry("ST5 : Please power cycle the Motor Drive and try connecting again", true);
return(false);
}
_serialPort.WriteLine("PR5");
CheckAck();
_serialPort.WriteLine("SKD"); //Stop and Kill
CheckAck();
_serialPort.WriteLine("MV");
string mvRead = _serialPort.ReadLine();
try
{
_model = (SERVO_MOTOR_MODEL)Convert.ToInt32(mvRead.Substring(4, 3));;
}
catch (Exception /*ex*/)
{
App.LogEntry.AddEntry("Unsupported Motor Drive", true);
return(false);
}
App.LogEntry.AddEntry("ST5 Model/Revision: " + mvRead);
_serialPort.WriteLine("OP");
App.LogEntry.AddEntry("ST5 Option Board: " + _serialPort.ReadLine());
_serialPort.WriteLine("SC");
string response = _serialPort.ReadLine().Substring(3);
App.LogEntry.AddEntry("ST5 Status: 0x" + response);
_motorEnabled = (Convert.ToInt32(response.Substring(3, 1)) % 2) == 1;
_driveBusy = (Convert.ToInt32(response.Substring(2, 1)) % 2) != 0;
Velocity = Properties.Settings.Default.MotorVelocity;
ContinuousVelocity = Properties.Settings.Default.MotorContinuousVelocity;
_serialPort.DiscardInBuffer();
_serialPort.DiscardOutBuffer();
ResetPosition();
_statusTimer = new Timer();
_statusTimer.Interval = 50;
_statusTimer.AutoReset = false;
_statusTimer.Elapsed += new ElapsedEventHandler(_statusTimer_Elapsed);
_driveConnected = true;
App.LogEntry.AddEntry("Connected to Stepper Motor");
break;
}
catch (Exception ex)
{
if (i == numRetries - 1)
{
App.LogEntry.AddEntry("Could not connect to Stepper Motor : " + ex.Message, true);
}
else
{
App.LogEntry.AddEntry("Could not connect to Stepper Motor : " + ex.Message);
}
_driveConnected = false;
System.Threading.Thread.Sleep(500);
}
}
return(_driveConnected);
}
bool SetSerialPort(SerialPort serialPort, MotorType motorType)
{
bool result = false;
try
{
//make sure in SCL mode
serialPort.WriteLine("PM");
if (serialPort.ReadLine().Substring(3) != "2")
{
serialPort.WriteLine("PM2");
//CheckAck();
App.LogEntry.AddEntry("ST5 : Please power cycle the Motor Drive and try connecting again", true);
return(false);
}
serialPort.WriteLine("PR5");
if (!CheckAck(serialPort))
{
return(false);
}
serialPort.WriteLine("SKD"); //Stop and Kill
if (!CheckAck(serialPort))
{
return(false);
}
serialPort.WriteLine("MV");
string mvRead = serialPort.ReadLine();
try
{
SERVO_MOTOR_MODEL _model = (SERVO_MOTOR_MODEL)Convert.ToInt32(mvRead.Substring(4, 3));;
}
catch (Exception /*ex*/)
{
App.LogEntry.AddEntry("Unsupported Motor Drive", true);
return(false);
}
App.LogEntry.AddEntry("ST5 Model/Revision: " + mvRead);
serialPort.WriteLine("OP");
App.LogEntry.AddEntry("ST5 Option Board: " + serialPort.ReadLine());
serialPort.WriteLine("SC");
string response = serialPort.ReadLine().Substring(3);
App.LogEntry.AddEntry("ST5 Status: 0x" + response);
bool _motorEnabled = (Convert.ToInt32(response.Substring(3, 1)) % 2) == 1;
bool _driveBusy = (Convert.ToInt32(response.Substring(2, 1)) % 2) != 0;
serialPort.WriteLine("AC100");
if (!CheckAck(serialPort))
{
return(false);
}
serialPort.WriteLine("DC100");
if (!CheckAck(serialPort))
{
return(false);
}
result = true;
}
catch (Exception ex)
{
Console.WriteLine("Failed in SetSerialPort: " + motorType.ToString() + " exception: " + ex.Message);
result = false;
}
return(result);
}
