// Inquire high speed ratio ":g(*2)", where *2: '1'= CH1, '2' = CH2.
protected void InquireHighSpeedRatio(AXISID Axis)
{
string response = TalkWithAxis(Axis, 'g', null);
long highSpeedRatio = BCDstr2long(response);
HighSpeedRatio[(int)Axis] = highSpeedRatio;
}
/************************ MOTOR COMMAND SET ***************************/
// Inquire Motor Board Version ":e(*1)", where *1: '1'= CH1, '2'= CH2, '3'= Both.
protected void InquireMotorBoardVersion(AXISID Axis)
{
string response = TalkWithAxis(Axis, 'e', null);
long tmpMCVersion = BCDstr2long(response);
MCVersion = ((tmpMCVersion & 0xFF) << 16) | ((tmpMCVersion & 0xFF00)) | ((tmpMCVersion & 0xFF0000) >> 16);
}
// Inquire PEC Period ":s(*1)", where *1: '1'= CH1, '2'= CH2, '3'= Both.
protected void InquirePECPeriod(AXISID Axis)
{
string response = TalkWithAxis(Axis, 's', null);
long PECPeriod = BCDstr2long(response);
PESteps[(int)Axis] = PECPeriod;
}
} // Return the mount's info
/// <summary>
/// One communication between mount and client
/// </summary>
/// <param name="Axis">The target of command</param>
/// <param name="Command">The comamnd char set</param>
/// <param name="cmdDataStr">The data need to send</param>
/// <returns>The response string from mount</returns>
protected virtual String TalkWithAxis(AXISID Axis, char Command, string cmdDataStr)
{
/// Lock the serial connection
/// It grantee there is only one thread entering this function in one time
/// ref: http://msdn.microsoft.com/en-us/library/ms173179.aspx
/// TODO: handle exception
lock (mConnection)
{
for (int i = 0; i < 2; i++)
{
/// The General Process for SerialPort COM
/// 1. Prepare Command Str by protocol
/// 2. Wait CTS if need
/// 3. Set RTS
/// 4. Send Command
/// 5. Receive Response
/// 6. Clear RTS
// prepare to communicate
try
{
mConnection.ClearBuffer();
mConnection.WaitIdle();
mConnection.Lock();
// send the request
SendRequest(Axis, Command, cmdDataStr);
// Release the line, so the mount can send response
mConnection.Release();
//Trace.TraceInformation("Send command successful");
// receive the response
return(RecieveResponse());
}
catch (TimeoutException e)
{
Trace.TraceError("Timeout, need Resend the Command");
}
catch (IOException e)
{
Trace.TraceError("Connnection Lost");
throw new MountControlException(ErrorCode.ERR_NOT_CONNECTED, e.Message);
}
}
//Trace.TraceError("Timeout, stop send");
if (Axis == AXISID.AXIS1)
{
throw new MountControlException(ErrorCode.ERR_NORESPONSE_AXIS1);
}
else
{
throw new MountControlException(ErrorCode.ERR_NORESPONSE_AXIS2);
}
}
}
// Inquire Timer Interrupt Freq ":b1".
protected void InquireTimerInterruptFreq(AXISID Axis)
{
string response = TalkWithAxis(Axis, 'b', null);
long TimeFreq = BCDstr2long(response);
StepTimerFreq[(int)Axis] = TimeFreq;
FactorRadRateToInt[(int)Axis] = (double)(StepTimerFreq[(int)Axis]) / FactorRadToStep[(int)Axis];
}
public override double MCGetAxisPosition(AXISID Axis)
{
string response = TalkWithAxis(Axis, 'j', null);
long iPosition = BCDstr2long(response);
iPosition -= 0x00800000;
Positions[(int)Axis] = StepToAngle(Axis, iPosition);
return(Positions[(int)Axis]);
}
public override void MCAxisSlewTo(AXISID Axis, double TargetPosition)
{
// Get current position of the axis.
var CurPosition = MCGetAxisPosition(Axis);
// Calculate slewing distance.
// Note: For EQ mount, Positions[AXIS1] is offset( -PI/2 ) adjusted in UpdateAxisPosition().
var MovingAngle = TargetPosition - CurPosition;
// Convert distance in radian into steps.
var MovingSteps = AngleToStep(Axis, MovingAngle);
bool forward = false, highspeed = false;
// If there is no increment, return directly.
if (MovingSteps == 0)
{
return;
}
// Set moving direction
if (MovingSteps > 0)
{
dir = '0';
forward = true;
}
else
{
dir = '1';
MovingSteps = -MovingSteps;
forward = false;
}
// Might need to check whether motor has stopped.
// Check if the distance is long enough to trigger a high speed GOTO.
if (MovingSteps > LowSpeedGotoMargin[(int)Axis])
{
SetMotionMode(Axis, '0', dir); // high speed GOTO slewing
highspeed = true;
}
else
{
SetMotionMode(Axis, '2', dir); // low speed GOTO slewing
highspeed = false;
}
SetGotoTargetIncrement(Axis, MovingSteps);
SetBreakPointIncrement(Axis, BreakSteps[(int)Axis]);
StartMotion(Axis);
TargetPositions[(int)Axis] = TargetPosition;
AxesStatus[(int)Axis].SetSlewingTo(forward, highspeed);
}
public override void MCSetAxisPosition(AXISID Axis, double NewValue)
{
long NewStepIndex = AngleToStep(Axis, NewValue);
NewStepIndex += 0x800000;
string szCmd = longTo6BitHEX(NewStepIndex);
TalkWithAxis(Axis, 'E', szCmd);
Positions[(int)Axis] = NewValue;
}
public override void MCAxisStop(AXISID Axis)
{
if (InstantStop)
{
TalkWithAxis(Axis, 'L', null);
}
else
{
TalkWithAxis(Axis, 'K', null);
}
AxesStatus[(int)Axis].SetFullStop();
}
public override AXISSTATUS MCGetAxisStatus(AXISID Axis)
{
var response = TalkWithAxis(Axis, 'f', null);
if ((response[2] & 0x01) != 0)
{
// Axis is running
if ((response[1] & 0x01) != 0)
{
AxesStatus[(int)Axis].Slewing = true; // Axis in slewing(AstroMisc speed) mode.
}
else
{
AxesStatus[(int)Axis].SlewingTo = true; // Axis in SlewingTo mode.
}
}
else
{
AxesStatus[(int)Axis].FullStop = true; // FullStop = 1; // Axis is fully stop.
}
if ((response[1] & 0x02) == 0)
{
AxesStatus[(int)Axis].SlewingForward = true; // Angle increase = 1;
}
else
{
AxesStatus[(int)Axis].SlewingForward = false;
}
if ((response[1] & 0x04) != 0)
{
AxesStatus[(int)Axis].HighSpeed = true; // HighSpeed running mode = 1;
}
else
{
AxesStatus[(int)Axis].HighSpeed = false;
}
if ((response[3] & 1) == 0)
{
AxesStatus[(int)Axis].NotInitialized = true; // MC is not initialized.
}
else
{
AxesStatus[(int)Axis].NotInitialized = false;
}
return(AxesStatus[(int)Axis]);
}
protected override void SendRequest(AXISID Axis, char Command, string cmdDataStr)
{
if (cmdDataStr == null)
cmdDataStr = "";
const int BufferSize = 20;
StringBuilder CommandStr = new StringBuilder(BufferSize);
CommandStr.Append(cStartChar_Out); // 0: Leading char
CommandStr.Append(Command); // 1: Length of command( Source, distination, command char, data )
// Target Device
CommandStr.Append(Axis == AXISID.AXIS1 ? '1' : '2'); // 2: Target Axis
// Copy command data to buffer
CommandStr.Append(cmdDataStr);
CommandStr.Append(cEndChar); // CR Character
mConnection.Write(CommandStr.ToString());
}
// Inquire Grid Per Revolution ":a(*2)", where *2: '1'= CH1, '2' = CH2.
protected void InquireGridPerRevolution(AXISID Axis)
{
string response = TalkWithAxis(Axis, 'a', null);
long GearRatio = BCDstr2long(response);
// There is a bug in the earlier version firmware(Before 2.00) of motor controller MC001.
// Overwrite the GearRatio reported by the MC for 80GT mount and 114GT mount.
if ((MCVersion & 0x0000FF) == 0x80)
{
GearRatio = 0x162B97; // for 80GT mount
}
if ((MCVersion & 0x0000FF) == 0x82)
{
GearRatio = 0x205318; // for 114GT mount
}
FactorRadToStep[(int)Axis] = GearRatio / (2 * Math.PI);
FactorStepToRad[(int)Axis] = 2 * Math.PI / GearRatio;
}
protected override void SendRequest(AXISID Axis, char Command, string cmdDataStr)
{
if (cmdDataStr == null)
{
cmdDataStr = "";
}
const int BufferSize = 20;
StringBuilder CommandStr = new StringBuilder(BufferSize);
CommandStr.Append(cStartChar_Out); // 0: Leading char
CommandStr.Append(Command); // 1: Length of command( Source, distination, command char, data )
// Target Device
CommandStr.Append(Axis == AXISID.AXIS1 ? '1' : '2'); // 2: Target Axis
// Copy command data to buffer
CommandStr.Append(cmdDataStr);
CommandStr.Append(cEndChar); // CR Character
mConnection.Write(CommandStr.ToString());
}
// Support Mount Status; public abstract AXISSTATUS MCGetAxisStatus(AXISID Axis);
// Unit: radian public abstract void MCSetAxisPosition(AXISID Axis, double pos);
public abstract void MCAxisSlewTo(AXISID Axis, double pos);
/// <summary> /// /// </summary> /// <exception cref="IOException">Throw when</exception> /// <param name="Axis"></param> /// <param name="Command"></param> /// <param name="cmdDataStr"></param> protected abstract void SendRequest(AXISID Axis, char Command, string cmdDataStr);
public override void MCSetAxisPosition(AXISID Axis, double NewValue)
{
Pos = new double[] { 0, 0 };
//throw new NotImplementedException();
}
public static AXISSTATUS GetAxisStatus(AXISID id)
{
return id == AXISID.AXIS1 ? AxisStatus1 : AxisStatus2;
}
public static void AxisSlewTo(AXISID id, double position)
{
pMount.MCAxisSlewTo(id, position * RAD1);
}
protected double[] FactorRadToStep = new double[] { 0, 0 }; // 將弧度數值乘以該系數即可得到馬達板的位置數值（24位數則丟棄最高字節即可）
protected long AngleToStep(AXISID Axis, double AngleInRad)
{
return (long)(AngleInRad * FactorRadToStep[(int)Axis]);
}
// Support Mount Status; public abstract AXISSTATUS MCGetAxisStatus(AXISID Axis);
public abstract double MCGetAxisPosition(AXISID Axis);
// Unit: radian public abstract void MCSetAxisPosition(AXISID Axis, double pos);
public abstract void MCAxisStop(AXISID Axis);
public abstract void MCAxisSlewTo(AXISID Axis, double pos);
// Mount dependent motion control functions public abstract void MCAxisSlew(AXISID Axis, double rad);
protected double[] FactorStepToRad = new double[] { 0, 0 }; // 將馬達板的位置數值(需處理符號問題后)乘以該系數即可得到弧度數值
protected double StepToAngle(AXISID Axis, long Steps)
{
return(Steps * FactorStepToRad[(int)Axis]);
}
private static int _ID(AXISID id)
{
return id == AXISID.AXIS1 ? 0 : 1;
}
protected double[] FactorStepToRad = new double[] { 0, 0 }; // 將馬達板的位置數值(需處理符號問題后)乘以該系數即可得到弧度數值
protected double StepToAngle(AXISID Axis, long Steps)
{
return Steps * FactorStepToRad[(int)Axis];
}
public static void SetAxisPosition(AXISID id, double NewValue)
{
pMount.MCSetAxisPosition(id, NewValue * RAD1);
}
protected double[] FactorRadRateToInt = new double[] { 0, 0 }; // 將弧度/秒數值乘以該系數即可得到馬達板所使用的設定速度的32位整數
protected long RadSpeedToInt(AXISID Axis, double RateInRad)
{
return (long)(RateInRad * FactorRadRateToInt[(int)Axis]);
}
public override void MCAxisSlewTo(AXISID Axis, double TargetPosition)
{
Pos[(int)Axis] = TargetPosition;
}
protected void StartMotion(AXISID Axis)
{
TalkWithAxis(Axis, 'J', null);
}
public override AXISSTATUS MCGetAxisStatus(AXISID Axis)
{
return(new AXISSTATUS {
FullStop = true
});
}
protected void SetStepPeriod(AXISID Axis, long StepsCount)
{
string szCmd = longTo6BitHEX(StepsCount);
TalkWithAxis(Axis, 'I', szCmd);
}
// Mount dependent motion control functions public abstract void MCAxisSlew(AXISID Axis, double rad);
protected void SetBreakSteps(AXISID Axis, long NewBrakeSteps)
{
string szCmd = longTo6BitHEX(NewBrakeSteps);
TalkWithAxis(Axis, 'U', szCmd);
}
public abstract void MCAxisStop(AXISID Axis);
protected void SetBreakPointIncrement(AXISID Axis, long StepsCount)
{
string szCmd = longTo6BitHEX(StepsCount);
TalkWithAxis(Axis, 'M', szCmd);
}
public abstract double MCGetAxisPosition(AXISID Axis);
protected void SetGotoTargetIncrement(AXISID Axis, long StepsCount)
{
string cmd = longTo6BitHEX(StepsCount);
TalkWithAxis(Axis, 'H', cmd);
}
protected double[] FactorRadToStep = new double[] { 0, 0 }; // 將弧度數值乘以該系數即可得到馬達板的位置數值（24位數則丟棄最高字節即可）
protected long AngleToStep(AXISID Axis, double AngleInRad)
{
return((long)(AngleInRad * FactorRadToStep[(int)Axis]));
}
protected void SetMotionMode(AXISID Axis, char func, char direction)
{
string szCmd = "" + func + direction;
TalkWithAxis(Axis, 'G', szCmd);
}
protected double[] FactorRadRateToInt = new double[] { 0, 0 }; // 將弧度/秒數值乘以該系數即可得到馬達板所使用的設定速度的32位整數
protected long RadSpeedToInt(AXISID Axis, double RateInRad)
{
return((long)(RateInRad * FactorRadRateToInt[(int)Axis]));
}
/************************ MOTOR COMMAND SET ***************************/
// Inquire Motor Board Version ":e(*1)", where *1: '1'= CH1, '2'= CH2, '3'= Both.
protected void InquireMotorBoardVersion(AXISID Axis)
{
string response = TalkWithAxis(Axis, 'e', null);
long tmpMCVersion = BCDstr2long(response);
MCVersion = ((tmpMCVersion & 0xFF) << 16) | ((tmpMCVersion & 0xFF00)) | ((tmpMCVersion & 0xFF0000) >> 16);
}
public static void AxisSlew(AXISID id, double degree)
{
pMount.MCAxisSlew(id, degree * RAD1);
}
// Inquire PEC Period ":s(*1)", where *1: '1'= CH1, '2'= CH2, '3'= Both.
protected void InquirePECPeriod(AXISID Axis)
{
string response = TalkWithAxis(Axis, 's', null);
long PECPeriod = BCDstr2long(response);
PESteps[(int)Axis] = PECPeriod;
}
public static void AxisStop(AXISID id)
{
pMount.MCAxisStop(id);
}
// Inquire high speed ratio ":g(*2)", where *2: '1'= CH1, '2' = CH2.
protected void InquireHighSpeedRatio(AXISID Axis)
{
string response = TalkWithAxis(Axis, 'g', null);
long highSpeedRatio = BCDstr2long(response);
HighSpeedRatio[(int)Axis] = highSpeedRatio;
}
public static double GetAxisPosition(AXISID id)
{
return id == AXISID.AXIS1 ? AxisPos1 : AxisPos2;
}
// Inquire Timer Interrupt Freq ":b1".
protected void InquireTimerInterruptFreq(AXISID Axis)
{
string response = TalkWithAxis(Axis, 'b', null);
long TimeFreq = BCDstr2long(response);
StepTimerFreq[(int)Axis] = TimeFreq;
FactorRadRateToInt[(int)Axis] = (double)(StepTimerFreq[(int)Axis]) / FactorRadToStep[(int)Axis];
}
public override void MCAxisSlew(AXISID Axis, double Speed)
{
//throw new NotImplementedException();
}
private string TalkWithAxis(AXISID axis, char cmd, string cmdDataStr)
{
string response = string.Empty;
DeviceEndpoint endPoint = SerialDeviceEndpoint.FromConnectionString(ConnectionString);
const int BufferSize = 20;
StringBuilder sb = new StringBuilder(BufferSize);
sb.Append(cStartChar_Out); // 0: Leading char
sb.Append(cmd); // 1: Length of command( Source, distination, command char, data )
// Target Device
sb.Append(((int)axis + 1).ToString()); // 2: Target Axis
// Copy command data to buffer
sb.Append(cmdDataStr);
sb.Append(cEndChar); // CR Character
string cmdString = sb.ToString();
var cmdTransaction = new EQTransaction(cmdString)
{
Timeout = TimeSpan.FromSeconds(TimeOut)
};
using (ICommunicationChannel channel = new SerialCommunicationChannel(endPoint))
using (var processor = new ReactiveTransactionProcessor())
{
var transactionObserver = new TransactionObserver(channel);
processor.SubscribeTransactionObserver(transactionObserver);
try
{
channel.Open();
// prepare to communicate
for (int i = 0; i < Retry; i++)
{
Task.Run(() => processor.CommitTransaction(cmdTransaction));
cmdTransaction.WaitForCompletionOrTimeout();
if (!cmdTransaction.Failed)
{
response = cmdTransaction.Value;
break;
}
else
{
Trace.TraceError(cmdTransaction.ErrorMessage.Single());
}
}
}
catch (Exception ex)
{
Trace.TraceError("Connnection Lost");
throw new Exception("AstroEQ not responding", ex);
}
finally
{
// To clean up, we just need to dispose the TransactionObserver and the channel is closed automatically.
// Not strictly necessary, but good practice.
transactionObserver.OnCompleted(); // There will be no more transactions.
transactionObserver = null; // not necessary, but good practice.
endPoint = null;
}
}
return(response);
}
public override void MCAxisStop(AXISID Axis)
{
//throw new NotImplementedException();
}
// Inquire Grid Per Revolution ":a(*2)", where *2: '1'= CH1, '2' = CH2.
protected void InquireGridPerRevolution(AXISID Axis)
{
string response = TalkWithAxis(Axis, 'a', null);
long GearRatio = BCDstr2long(response);
// There is a bug in the earlier version firmware(Before 2.00) of motor controller MC001.
// Overwrite the GearRatio reported by the MC for 80GT mount and 114GT mount.
if ((MCVersion & 0x0000FF) == 0x80)
{
GearRatio = 0x162B97; // for 80GT mount
}
if ((MCVersion & 0x0000FF) == 0x82)
{
GearRatio = 0x205318; // for 114GT mount
}
FactorRadToStep[(int)Axis] = GearRatio / (2 * Math.PI);
FactorStepToRad[(int)Axis] = 2 * Math.PI / GearRatio;
}
public override double MCGetAxisPosition(AXISID Axis)
{
return(Pos[(int)Axis]);
}
/// <summary> /// /// </summary> /// <exception cref="IOException">Throw when</exception> /// <param name="Axis"></param> /// <param name="Command"></param> /// <param name="cmdDataStr"></param> protected abstract void SendRequest(AXISID Axis, char Command, string cmdDataStr);
protected override void SendRequest(AXISID Axis, char Command, string cmdDataStr)
{
throw new NotImplementedException();
}
public virtual MOUNTTYPE DetectMount() { return null; } // Return the mount's info
/// <summary>
/// One communication between mount and client
/// </summary>
/// <param name="Axis">The target of command</param>
/// <param name="Command">The comamnd char set</param>
/// <param name="cmdDataStr">The data need to send</param>
/// <returns>The response string from mount</returns>
protected virtual String TalkWithAxis(AXISID Axis, char Command, string cmdDataStr)
{
/// Lock the serial connection
/// It grantee there is only one thread entering this function in one time
/// ref: http://msdn.microsoft.com/en-us/library/ms173179.aspx
/// TODO: handle exception
lock (mConnection)
{
for (int i = 0; i < 2; i++)
{
/// The General Process for SerialPort COM
/// 1. Prepare Command Str by protocol
/// 2. Wait CTS if need
/// 3. Set RTS
/// 4. Send Command
/// 5. Receive Response
/// 6. Clear RTS
// prepare to communicate
try
{
mConnection.ClearBuffer();
mConnection.WaitIdle();
mConnection.Lock();
// send the request
SendRequest(Axis, Command, cmdDataStr);
// Release the line, so the mount can send response
mConnection.Release();
//Trace.TraceInformation("Send command successful");
// receive the response
return RecieveResponse();
}
catch (TimeoutException e)
{
Trace.TraceError("Timeout, need Resend the Command");
}
catch (IOException e)
{
Trace.TraceError("Connnection Lost");
throw new MountControlException(ErrorCode.ERR_NOT_CONNECTED, e.Message);
}
}
//Trace.TraceError("Timeout, stop send");
if (Axis == AXISID.AXIS1)
throw new MountControlException(ErrorCode.ERR_NORESPONSE_AXIS1);
else
throw new MountControlException(ErrorCode.ERR_NORESPONSE_AXIS2);
}
}
