public static Matrix3x3 Rotate(float angleInDeg, AxisId axisId)
{
float andleInRad = Mathf.Deg2Rad * angleInDeg;
float cos = Mathf.Cos(andleInRad);
float sin = Mathf.Sin(andleInRad);
float[,] matrix = new float[3, 3];
Array.Clear(matrix, 0, 9);
if (axisId == AxisId.Right)
{
matrix[0, 0] = 1;
matrix[1, 1] = cos; matrix[1, 2] = -sin;
matrix[2, 1] = sin; matrix[2, 2] = cos;
}
else if (axisId == AxisId.Up)
{
matrix[1, 1] = 1;
matrix[0, 0] = cos; matrix[0, 2] = sin;
matrix[2, 0] = -sin; matrix[2, 2] = cos;
}
else
{
matrix[2, 2] = 1;
matrix[0, 0] = cos; matrix[0, 1] = -sin;
matrix[1, 0] = sin; matrix[1, 1] = cos;
}
return(new Matrix3x3(matrix));
}
/// <summary>
/// i Get Current "slew" speed
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
internal long GetLastSlewSpeed(AxisId axis)
{
var response = CmdToAxis(axis, 'i', null);
var responseString = StringToLong(response);
return(responseString);
}
///// <summary>
///// get current step count
///// </summary>
///// <param name="axis"></param>
///// <returns></returns>
//private int GetEncoderCount(AxisId axis)
//{
// var stepsSky = new SkyGetEncoderCount(SkyQueue.NewId, axis);
// var steps = (int[])SkyQueue.GetCommandResult(stepsSky).Result;
// if (!stepsSky.Successful && stepsSky.Exception != null) throw stepsSky.Exception;
// switch (axis)
// {
// case AxisId.Axis1:
// return steps[0];
// case AxisId.Axis2:
// return steps[1];
// default:
// throw new ArgumentOutOfRangeException(nameof(axis), axis, null);
// }
//}
/// <summary>
/// Gets the direction to home sensor or if null then TripPosition was set
/// </summary>
/// <param name="axis"></param>
/// <returns></returns>
private bool?GetHomeSensorStatus(AxisId axis)
{
var sensorStatusSky = new SkyGetHomePosition(SkyQueue.NewId, axis);
var sensorStatus = (long)SkyQueue.GetCommandResult(sensorStatusSky).Result;
var monitorItem = new MonitorEntry
{
Datetime = HiResDateTime.UtcNow,
Device = MonitorDevice.Telescope,
Category = MonitorCategory.Server,
Type = MonitorType.Information,
Method = MethodBase.GetCurrentMethod().Name,
Thread = Thread.CurrentThread.ManagedThreadId,
Message = $"{sensorStatus}"
};
MonitorLog.LogToMonitor(monitorItem);
switch (sensorStatus)
{
case 16777215:
return(false);
case 0:
return(true);
default:
TripPosition = Convert.ToInt32(sensorStatus);
return(null);
}
}
/// <summary>
/// h Get Current "goto" target
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
internal double GetLastGoToTarget(AxisId axis)
{
var response = CmdToAxis(axis, 'h', null);
var responseString = StringToLong(response);
return(responseString);
}
/// <summary>
/// f Get the target axis's status as a struct
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <returns></returns>
internal AxisStatus GetAxisStatus(AxisId axis)
{
var response = CmdToAxis(axis, 'f', null);
//check if at full stop = 1
if ((response[2] & 0x01) == 0)
{
_axesStatus[(int)axis].FullStop = true;
_axesStatus[(int)axis].Slewing = false;
_axesStatus[(int)axis].SlewingTo = false;
_axesStatus[(int)axis].StepSpeed = "*";
}
else
{
// Axis is running
_axesStatus[(int)axis].FullStop = false;
_axesStatus[(int)axis].Slewing = (response[1] & 0x01) != 0;
_axesStatus[(int)axis].SlewingTo = (response[1] & 0x01) == 0;
}
_axesStatus[(int)axis].SlewingForward = (response[1] & 0x02) == 0;
_axesStatus[(int)axis].HighSpeed = (response[1] & 0x04) != 0;
_axesStatus[(int)axis].NotInitialized = (response[3] & 1) == 0;
//_axesStatus[(int)axis].StepSpeed = GetLastSlewSpeed(axis).ToString();
return(_axesStatus[(int)axis]);
}
/// <summary>
/// g Inquire High Speed Ratio, EQ6Pro, AZeQ5, EQ8 = 16 AZeQ6 = 32
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
private void GetHighSpeedRatio(AxisId axis)
{
var response = CmdToAxis(axis, 'g', null);
var highSpeedRatio = StringToLong(response);
_highSpeedRatio[(int)axis] = highSpeedRatio;
}
/// <summary>
/// c micro steps from target where the ramp down process begins
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
internal double GetRampDownRange(AxisId axis)
{
var response = CmdToAxis(axis, 'c', null);
var responseString = StringToLong(response);
return(responseString);
}
/// <summary>
/// d Get Current Encoder count
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
internal double GetEncoderCount(AxisId axis)
{
var response = CmdToAxis(axis, 'd', null);
var responseString = StringToLong(response);
return(responseString);
}
/// <summary>
/// a Inquire Grid Per Revolution ":a(*2)", where *2: '1'= CH1, '2' = CH2.
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
private void GetStepsPerRevolution(AxisId axis)
{
var response = CmdToAxis(axis, 'a', null);
var gearRatio = StringToLong(response);
// There is a issue 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 (axis == AxisId.Axis1)
{
if ((_axisVersion[0] & 0x0000FF) == 0x80)
{
gearRatio = 0x162B97; // for 80GT mount
}
if ((_axisGearRatios[0] & 0x0000FF) == 0x82)
{
gearRatio = 0x205318; // for 114GT mount
}
_axisGearRatios[0] = gearRatio;
}
else
{
if ((_axisVersion[1] & 0x0000FF) == 0x80)
{
gearRatio = 0x162B97; // for 80GT mount
}
if ((_axisVersion[1] & 0x0000FF) == 0x82)
{
gearRatio = 0x205318; // for 114GT mount
}
_axisGearRatios[1] = gearRatio;
}
_factorRadToStep[(int)axis] = gearRatio / (2 * Math.PI);
_factorStepToRad[(int)axis] = 2 * Math.PI / gearRatio;
}
/// <summary>
/// qx01 Capabilities
/// :qx010000[0D]=ABCDEF[0D] ie the bit mapped nybbles for current status
/// A 8 not defined
/// 4 not defined
/// 2 pPEC ON
/// 1 pPEC training in progress,
/// B 8 supports AZ/EQ
/// 4 has Home Sensors
/// 2 supports pPEC
/// 1 supports dual encoders
/// C 8 has WIFI
/// 4 supports half current tracking // ref :Wx06....
/// 2 axes slews must start independently // ie cant use :J3
/// 1 has polar LED
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
internal string GetCapabilities(AxisId axis)
{
var szCmd = LongToHex(1);
var response = CmdToAxis(axis, 'q', szCmd);
return(response);
}
/// <summary>
/// Converts Angle to steps
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <param name="angleInRad"></param>
/// <returns></returns>
internal long AngleToStep(AxisId axis, double angleInRad)
{
//var a = (long)(angleInRad * _factorRadToStep[(int)axis]);
var a = (long)Math.Floor(angleInRad * _factorRadToStep[(int)axis]);
return(a);
}
public Matrix3x3 Rotate(float angleInDeg, AxisId axis)
{
float angleInRad = Mathf.Deg2Rad * angleInDeg;
switch (axis)
{
case AxisId.x:
return(new Matrix3x3(1, 0, 0,
0, Mathf.Cos(angleInRad), Mathf.Sin(angleInRad) * -1,
0, Mathf.Sin(angleInRad), Mathf.Cos(angleInRad)
));
case AxisId.y:
return(new Matrix3x3(Mathf.Cos(angleInRad), 0, Mathf.Sin(angleInRad),
0, 1, 0,
Mathf.Sin(angleInRad) * -1, 0, Mathf.Cos(angleInRad)
));
case AxisId.z:
return(new Matrix3x3(Mathf.Cos(angleInRad), Mathf.Sin(angleInRad) * -1, 0,
Mathf.Sin(angleInRad), Mathf.Cos(angleInRad), 0,
0, 0, 1));
}
return(new Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0));
}
private string GetTalkWithAxisCommand(AxisId axis, char cmd, string cmdDataStr)
{
//System.Diagnostics.Debug.WriteLine(String.Format("TalkWithAxis({0}, {1}, {2})", axis, cmd, cmdDataStr));
string response = string.Empty;
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();
//System.Diagnostics.Debug.WriteLine($" - > Command: {cmdString}");
//string.Format("{0}{1}{2}{3}{4}",
//cStartChar_Out,
//command,
//(int)axis,
//(cmdDataStr ?? "."),
//cEndChar);
return(cmdString);
}
public void TestGotoStates()
{
AxisId axis = AxisId.Axis1_RA;
string response;
int state;
_Controller.MCAxisSlewTo(axis, 0.25 * CoreConstants.DEG_RAD, HemisphereOption.Northern);
//System.Diagnostics.Debug.WriteLine("Goto Forward Low Speed");
//System.Diagnostics.Debug.WriteLine(_Controller.TalkWithAxis(axis, 'f', null));
response = _Controller.TalkWithAxis(axis, 'f', null);
state = Convert.ToInt32(response.Substring(1, response.Length - 2), 16);
AxisState axisState = _Controller.MCGetAxisState(axis);
while (!axisState.FullStop)
{
Thread.Sleep(100);
axisState = _Controller.MCGetAxisState(axis);
}
Assert.AreEqual(0x011, state);
_Controller.MCAxisSlewTo(axis, 30.0 * CoreConstants.DEG_RAD, HemisphereOption.Northern);
System.Diagnostics.Debug.WriteLine("Goto Forward High Speed");
//System.Diagnostics.Debug.WriteLine(_Controller.TalkWithAxis(axis, 'f', null));
response = _Controller.TalkWithAxis(axis, 'f', null);
state = Convert.ToInt32(response.Substring(1, response.Length - 2), 16);
axisState = _Controller.MCGetAxisState(axis);
while (!axisState.FullStop)
{
Thread.Sleep(100);
axisState = _Controller.MCGetAxisState(axis);
}
Assert.AreEqual(0x411, state);
_Controller.MCAxisSlewTo(axis, 0.25 * CoreConstants.DEG_RAD, HemisphereOption.Northern);
System.Diagnostics.Debug.WriteLine("Goto Reverse High Speed");
//System.Diagnostics.Debug.WriteLine(_Controller.TalkWithAxis(axis, 'f', null));
response = _Controller.TalkWithAxis(axis, 'f', null);
state = Convert.ToInt32(response.Substring(1, response.Length - 2), 16);
axisState = _Controller.MCGetAxisState(axis);
while (!axisState.FullStop)
{
Thread.Sleep(100);
axisState = _Controller.MCGetAxisState(axis);
}
Assert.AreEqual(0x611, state);
_Controller.MCAxisSlewTo(axis, 0.0 * CoreConstants.DEG_RAD, HemisphereOption.Northern);
System.Diagnostics.Debug.WriteLine("Goto Reverse Low Speed");
//System.Diagnostics.Debug.WriteLine(_Controller.TalkWithAxis(axis, 'f', null));
response = _Controller.TalkWithAxis(axis, 'f', null);
state = Convert.ToInt32(response.Substring(1, response.Length - 2), 16);
axisState = _Controller.MCGetAxisState(axis);
while (!axisState.FullStop)
{
Thread.Sleep(100);
axisState = _Controller.MCGetAxisState(axis);
}
Assert.AreEqual(0x211, state);
}
/// <summary>
/// j Gets axis position counter
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <returns>Cardinal encoder count</returns>
internal long GetAxisPositionCounter(AxisId axis)
{
var response = CmdToAxis(axis, 'j', null);
var iPosition = StringToLong(response);
iPosition -= 0x00800000;
return(iPosition);
}
private string EQ_SendGCode(AxisId axisId, HemisphereOption hemisphere, AxisMode mode, AxisDirection direction, AxisSpeed speed)
{
System.Diagnostics.Debug.WriteLine(string.Format("EQ_SendGCode({0}, {1}, {2}, {3}, {4})", axisId, hemisphere, mode, direction, speed));
byte ch;
ch = 0;
// Set Direction bit (Bit 0)
if (direction == AxisDirection.Reverse)
{
ch |= 0x01;
}
// Set Hemisphere bit (Bit 1)
if (hemisphere == HemisphereOption.Southern)
{
ch |= 0x02;
}
// 0 = high speed GOTO mode
// 1 = low speed SLEW mode
// 2 = low speed GOTO mode
// 3 = high speed SLEW mode
// Set Mode and speed bits
if (mode == AxisMode.Goto)
{
//goto
if (speed == AxisSpeed.LowSpeed)
{
// Low speed goto = 2
ch |= 0x20;
}
else
{
//high speed goto = 0
}
}
else
{
// slew
if (speed == AxisSpeed.HighSpeed)
{
// High speed slew= 3
ch |= 0x30;
}
else
{
// low speed slew= 1
ch |= 0x10;
}
}
// Send 'G' Command, with parameter
return(GetEQSendCommandCommand(axisId, 'G', ch, 2));
}
/// <summary>
/// j Gets radians position of an axis
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <returns>Radians of the axis</returns>
internal double GetAxisPosition(AxisId axis)
{
var response = CmdToAxis(axis, 'j', null);
var iPosition = StringToLong(response);
iPosition -= 0x00800000;
_positions[(int)axis] = StepToAngle(axis, iPosition);
return(_positions[(int)axis]);
}
/// <summary>
/// s Inquire PEC Period ":s(*1)", where *1: '1'= CH1, '2'= CH2, '3'= Both.
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
internal double GetPecPeriod(AxisId axis)
{
var response = CmdToAxis(axis, 's', null);
var pecPeriod = StringToLong(response);
_peSteps[(int)axis] = pecPeriod;
return(pecPeriod);
}
protected string MCSetMotionMode(AxisId axis, HemisphereOption hemisphere, AxisMode mode, AxisDirection direction, AxisSpeed speed)
{
byte ch;
ch = 0;
// Set Direction bit (Bit 0)
if (direction == AxisDirection.Reverse)
{
ch |= 0x01;
}
// Set Hemisphere bit (Bit 1)
if (hemisphere == HemisphereOption.Southern)
{
ch |= 0x02;
}
// 0 = high speed GOTO mode
// 1 = low speed SLEW mode
// 2 = low speed GOTO mode
// 3 = high speed SLEW mode
// Set Mode and speed bits
if (mode == AxisMode.Goto)
{
//goto
if (speed == AxisSpeed.LowSpeed)
{
// Low speed goto = 2
ch |= 0x20;
}
else
{
//high speed goto = 0
}
}
else
{
// slew
if (speed == AxisSpeed.HighSpeed)
{
// High speed slew= 3
ch |= 0x30;
}
else
{
// low speed slew= 1
ch |= 0x10;
}
}
string szCmd = LongTo2BitHEX(ch);
return(GetTalkWithAxisCommand(axis, 'G', szCmd));
}
/// <summary>
/// Wx06 Wx0601 on/off Full Current Low speed
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <param name="on"></param>
internal void SetLowSpeedCurrent(AxisId axis, bool on)
{
var szCmd = LongToHex(6);
if (on)
{
szCmd = "060100";
}
CmdToAxis(axis, 'W', szCmd);
}
/// <summary>
/// Wx04 Wx05 on/off encoders
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <param name="on"></param>
internal void SetEncoders(AxisId axis, bool on)
{
var szCmd = LongToHex(5);
if (on)
{
szCmd = LongToHex(4);
}
CmdToAxis(axis, 'W', szCmd);
}
/// <summary>
/// Wx03 on/off pPEC
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <param name="on"></param>
internal void SetPPec(AxisId axis, bool on)
{
var szCmd = LongToHex(3);
if (on)
{
szCmd = LongToHex(2);
}
CmdToAxis(axis, 'W', szCmd);
}
/// <summary>
/// Wx01 on/off pPEC train
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <param name="on"></param>
internal void SetPPecTrain(AxisId axis, bool on)
{
var szCmd = LongToHex(1);
if (on)
{
szCmd = LongToHex(0);
}
CmdToAxis(axis, 'W', szCmd);
}
/// <summary>
/// b Inquire Timer Interrupt Freq ":b1".
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
private void GetTimerInterruptFreq(AxisId axis)
{
var response = CmdToAxis(axis, 'b', null);
var timeFreq = StringToLong(response);
_stepTimerFreq[(int)axis] = timeFreq;
_factorRadRateToInt[(int)axis] = _stepTimerFreq[(int)axis] / _factorRadToStep[(int)axis];
}
private void DrawRotationTrail(float angle, AxisId axis, Vector3 start)
{
Vector3 trail = start;
for (int i = 0; i <= 16; i++)
{
Vector3 trailNextStep = Matrix3x3.Rotate(angle % 360 * i / 16f, axis) * start;
Gizmos.DrawLine(trail, trailNextStep);
trail = trailNextStep;
}
}
/// <summary>
/// E Set the target axis position to the specify value
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <param name="radians">radian value</param>
internal void SetAxisPosition(AxisId axis, double radians)
{
var newStepIndex = AngleToStep(axis, radians);
newStepIndex += 0x800000;
var szCmd = LongToHex(newStepIndex);
CmdToAxis(axis, 'E', szCmd);
_positions[(int)axis] = radians;
}
/// <summary>
/// qx00 Home position
/// Send :qx000000[0D]
/// =000000[0D] if axis is CW from home ( ie -ve ) just after home sensor trip has been reset
/// =FFFFFF[0D] CCW from home(ie +ve ) just after home sensor trip has been reset )
/// =llhhLL[0D] if sensor has tripped since reset(use :W to clear data first )
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
internal long GetHomePosition(AxisId axis)
{
var szCmd = LongToHex(0);
var response = CmdToAxis(axis, 'q', szCmd);
var position = StringToLong(response);
if (response == "=000000")
{
position = -position;
}
return(position);
}
/// <summary>
/// Slew to home based on TripPosition already being set
/// </summary>
/// <param name="axis"></param>
private int SlewToHome(AxisId axis)
{
if (SkyServer.AutoHomeStop)
{
return(-3); //stop requested
}
//convert position to mount degrees
var a = TripPosition -= 0x00800000;
var skyCmd = new SkyGetStepToAngle(SkyQueue.NewId, axis, a);
var b = (double)SkyQueue.GetCommandResult(skyCmd).Result;
var c = Units.Rad2Deg1(b);
var positions = Axes.MountAxis2Mount();
switch (axis)
{
case AxisId.Axis1:
var d = Axes.AxesMountToApp(new[] { c, 0 }); // Convert to local
if (SkyServer.SouthernHemisphere)
{
d[0] = d[0] + 180;
}
SkyServer.SlewAxes(d[0], positions[1], SlewType.SlewMoveAxis);
break;
case AxisId.Axis2:
var e = Axes.AxesMountToApp(new[] { 0, c }); // Convert to local
if (SkyServer.SouthernHemisphere)
{
e[1] = 180 - e[1];
}
SkyServer.SlewAxes(positions[0], e[1], SlewType.SlewMoveAxis);
break;
default:
throw new ArgumentOutOfRangeException(nameof(axis), axis, null);
}
while (SkyServer.IsSlewing)
{
//Console.WriteLine(@"slewing");
Thread.Sleep(300);
}
var _ = new SkyAxisStop(0, axis);
//Thread.Sleep(1500);
return(0);
}
public LdrBuilder Graph(string name, AxisId axis_id, bool smooth, IEnumerable <v4> data)
{
var bbox = BBox.Reset;
foreach (var d in data)
{
BBox.Grow(ref bbox, d);
}
using var grp = Group(name);
Grid(string.Empty, 0xFFAAAAAA, axis_id, (int)(bbox.SizeX * 1.1), (int)(bbox.SizeY * 1.1), 50, 50, new v4(bbox.Centre.x, bbox.Centre.y, 0f, 1f));
Line("data", 0xFFFFFFFF, 1, smooth, data);
return(this);
}
/// <summary>
/// j Gets radians position of an axis
/// </summary>
/// <param name="axis">AxisId.Axis1 or AxisId.Axis2</param>
/// <returns>Radians of the axis or NaN if no response is received</returns>
internal double GetAxisPositionNaN(AxisId axis)
{
var response = CmdToAxis(axis, 'j', null, true);
if (string.IsNullOrEmpty(response))
{
return(double.NaN);
}
var iPosition = StringToLong(response);
iPosition -= 0x00800000;
_positions[(int)axis] = StepToAngle(axis, iPosition);
return(_positions[(int)axis]);
}
