/// <summary>
/// Displays the position of servo 0 in the text box.
/// </summary>
private void DisplayPosition()
{
ServoStatus[] servos;
usc.getVariables(out servos);
PositionTextBox.Text = servos[0].position.ToString();
}
public int TryGetTarget(Byte channel)
{
int ret = 0;
try
{
int index = channel / ServoChannelMap.CHANNELS_PER_DEVICE;
channel = (byte)(channel % ServoChannelMap.CHANNELS_PER_DEVICE);
using (Usc device = connectToDevice(index)) // Find a device and temporarily connect.
{
ServoStatus[] servos;
device.getVariables(out servos);
ret = servos[channel].position;
// device.Dispose() is called automatically when the "using" block ends,
// allowing other functions and processes to use the device.
}
}
catch (Exception exception) // Handle exceptions by displaying them to the user.
{
Console.WriteLine(exception);
}
return(ret);
}
static unsafe void displayStatus(Usc usc)
{
MaestroVariables variables;
ServoStatus[] servos;
short[] stack;
ushort[] call_stack;
usc.getVariables(out variables, out stack, out call_stack, out servos);
Console.WriteLine(" # target speed accel pos");
for (int i = 0; i < usc.servoCount; i++)
{
Console.WriteLine("{0,2}{1,8}{2,8}{3,8}{4,8}", i,
servos[i].target, servos[i].speed,
servos[i].acceleration, servos[i].position);
}
Console.Write("errors: 0x"+variables.errors.ToString("X4"));
foreach (var error in Enum.GetValues(typeof(uscError)))
{
if((variables.errors & (1<<(int)(uscError)error)) != 0)
{
Console.Write(" "+error.ToString());
}
}
usc.clearErrors();
Console.WriteLine(); // end the line
if(variables.scriptDone == 0)
Console.WriteLine("SCRIPT RUNNING");
else
Console.WriteLine("SCRIPT DONE");
Console.WriteLine("program counter: "+variables.programCounter.ToString());
Console.Write("stack: ");
for (int i = 0; i < variables.stackPointer; i++)
{
if(i > stack.Length)
{
Console.Write(" OVERFLOW (stack pointer = "+variables.stackPointer+")");
break;
}
Console.Write(stack[i].ToString().PadLeft(8, ' '));
}
Console.WriteLine();
Console.Write("call stack: ");
for (int i = 0; i < variables.callStackPointer; i++)
{
if(i > call_stack.Length)
{
Console.Write(" OVERFLOW (call stack pointer = "+variables.callStackPointer+")");
break;
}
Console.Write(call_stack[i].ToString().PadLeft(8, ' '));
}
Console.WriteLine();
}
// Reads the servo status from the servo controller hardware, and then updates the
// servoList with these new polled values. Also tracks whether each servo is currently
// moving by comparing their current and target positions.
public void UpdateServoValues()
{
if (!IsConnected())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "UpdateServoValues() failed, not connected to servo hardware.");
return;
}
if (servoList.Count == 0)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "UpdateServoValues() failed, no servos have been defined.");
return;
}
try
{
// Get the servo parameters from the hardware.
ServoStatus[] servoStatusArray;
uscDevice.getVariables(out servoStatusArray);
// Update the servoList with these parameters.
foreach (Servo servo in servoList)
{
if (servo.index < 0 || servo.index >= uscDevice.servoCount)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "UpdateServoValues() failed, servo index out of range. Servo index = " + servo.index.ToString());
}
else
{
servo.polledPosition = servoStatusArray[servo.index].position;
servo.polledTarget = servoStatusArray[servo.index].target;
servo.polledSpeed = servoStatusArray[servo.index].speed;
servo.polledAcceleration = servoStatusArray[servo.index].acceleration;
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index.ToString() + ": Target = " + servo.polledTarget.ToString() + ", Position = " + servo.polledPosition.ToString() + ", Speed = " + servo.polledSpeed.ToString() + ", Acceleration = " + servo.polledAcceleration.ToString());
if (servo.isMoving == false && servo.polledTarget != servo.polledPosition)
{
// Servo has started moving.
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index + " has started moving from " + servo.polledPosition.ToString() + " to " + servo.polledTarget.ToString());
servo.isMoving = true;
}
else if (servo.isMoving == true && servo.polledTarget == servo.polledPosition)
{
// Servo has stopped moving.
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index + " has stopped moving at " + servo.polledPosition.ToString());
servo.isMoving = false;
}
if (servo.isMoving)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index + " is at position " + servo.polledPosition.ToString());
}
}
}
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in UpdateServoValues(): " + ex.Message);
}
}
// Pulls data from the devices and places it in the table created earlier.
public static void updateTable(DataGridView dataTable)
{
DataTable flippedStatusValues = new DataTable();
string period;
//
// PULLING DATA FROM MAESTRO
//
statusValues.Clear();
if (NumOfConnectedMaestros > 0)
{
usc.getVariables(out maestroVars, out stack, out call_stack, out servos);
errors = "";
foreach (var error in Enum.GetValues(typeof(uscError)))
{
if ((maestroVars.errors & (1 << (int)(uscError)error)) != 0)
{
errors += (error.ToString() + ", ");
}
}
//
//
// Need to add data information for other values, if needed
//
//
statusValues.Rows.Add("Maestro", usc.getSerialNumber(), errors, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
}
// Case for when there are no connected Maestros
else
{
statusValues.Rows.Add("Maestro", "No Maestros Found", "", -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
}
//
// PULLING DATA FROM JRK 1
//
if (Program.NumOfConnectedJrks > 0 && usc.servoCount > 0)
{
// Preparing Jrk data input
jrkVars = Jrk1.getVariables();
// Reading Jrk current
// Easier to store in a variable before sending to the table.
currentCalibrationForward = (Byte)Jrk1.getJrkParameter(jrkParameter.PARAMETER_MOTOR_CURRENT_CALIBRATION_FORWARD);
currentCalibrationReverse = (Byte)Jrk1.getJrkParameter(jrkParameter.PARAMETER_MOTOR_CURRENT_CALIBRATION_REVERSE);
current = currentToMilliamps(Jrk1, jrkVars.current, jrkVars.dutyCycle);
// Reading Jrk errors
// It's easier to store them all in one variable before sending to the table.
errors = "";
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_AWAITING_COMMAND))
{
errors += "Awaiting command, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_NO_POWER))
{
errors += "No power, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_MOTOR_DRIVER))
{
errors += "Motor driver error, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_INPUT_INVALID))
{
errors += "Input invalid, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_INPUT_DISCONNECT))
{
errors += "Input disconnect";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_FEEDBACK_DISCONNECT))
{
errors += "Feedback disconnect, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_MAXIMUM_CURRENT_EXCEEDED))
{
errors += "Max. current exceeded, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_SIGNAL))
{
errors += "Serial signal error, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_OVERRUN))
{
errors += "Serial overrun, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_BUFFER_FULL))
{
errors += "Serial RX buffer full, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_CRC))
{
errors += "Serial CRC error, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_PROTOCOL))
{
errors += "Serial protocol error, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_TIMEOUT))
{
errors += "Serial timeout error, ";
}
period = servos[0].position * 0.25 + " microseconds";
//Setting up the table with values
statusValues.Rows.Add("Jrk", Jrk1.getSerialNumber(), errors, servos[0].position, period, jrkVars.target, jrkVars.scaledFeedback, jrkVars.feedback,
jrkVars.errorSum, jrkVars.dutyCycleTarget, jrkVars.dutyCycle, servos[0].speed, servos[0].acceleration, current);
}
else
{
statusValues.Rows.Add("Jrk", "No Jrks Found");
}
//
// PULLING DATA FROM JRK 2
//
if (Program.NumOfConnectedJrks > 1)
{
//Preparing Jrk data input
jrkVars = Jrk2.getVariables();
//Reading Jrk current
// Easier to store in a variable before sending to the table.
currentCalibrationForward = (Byte)Jrk2.getJrkParameter(jrkParameter.PARAMETER_MOTOR_CURRENT_CALIBRATION_FORWARD);
currentCalibrationReverse = (Byte)Jrk2.getJrkParameter(jrkParameter.PARAMETER_MOTOR_CURRENT_CALIBRATION_REVERSE);
current = currentToMilliamps(Jrk2, jrkVars.current, jrkVars.dutyCycle);
//Reading Jrk errors
// It's easier to store them all in one variable before sending to the table.
errors = "";
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_AWAITING_COMMAND))
{
errors += "Awaiting command, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_NO_POWER))
{
errors += "No power, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_MOTOR_DRIVER))
{
errors += "Motor driver error, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_INPUT_INVALID))
{
errors += "Input invalid, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_INPUT_DISCONNECT))
{
errors += "Input disconnect";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_FEEDBACK_DISCONNECT))
{
errors += "Feedback disconnect, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_MAXIMUM_CURRENT_EXCEEDED))
{
errors += "Max. current exceeded, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_SIGNAL))
{
errors += "Serial signal error, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_OVERRUN))
{
errors += "Serial overrun, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_BUFFER_FULL))
{
errors += "Serial RX buffer full, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_CRC))
{
errors += "Serial CRC error, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_PROTOCOL))
{
errors += "Serial protocol error, ";
}
if (1 == (1 & jrkVars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_TIMEOUT))
{
errors += "Serial timeout error, ";
}
period = servos[1].position * 0.25 + " microseconds";
//Setting up the table with values
statusValues.Rows.Add("Jrk", Jrk1.getSerialNumber(), errors, servos[1].position, period, jrkVars.target, jrkVars.scaledFeedback, jrkVars.feedback,
jrkVars.errorSum, jrkVars.dutyCycleTarget, jrkVars.dutyCycle, servos[1].speed, servos[1].acceleration, current);
}
//Future extension to more devices
/*
* if (Program.NumOfConnectedJrks > 2)
* {
* }
* if (Program.NumOfConnectedJrks > 3)
* {
* }
*/
flippedStatusValues.Columns.Add("");
string deviceTrack = "";
for (int i = 0; i < statusValues.Rows.Count; i++)
{
deviceTrack = "Device " + i;
flippedStatusValues.Columns.Add(deviceTrack);
}
for (int i = 0; i < statusValues.Columns.Count; i++)
{
DataRow newRow = flippedStatusValues.NewRow();
newRow[0] = statusValues.Columns[i].Caption;
for (int j = 0; j < statusValues.Rows.Count; j++)
{
newRow[j + 1] = statusValues.Rows[j][i];
}
flippedStatusValues.Rows.Add(newRow);
}
// Configuring the table to show the data
dataTable.DataSource = flippedStatusValues;
}
static unsafe void displayStatus(Usc usc)
{
MaestroVariables variables;
ServoStatus[] servos;
short[] stack;
ushort[] call_stack;
usc.getVariables(out variables, out stack, out call_stack, out servos);
Console.WriteLine(" # target speed accel pos");
for (int i = 0; i < usc.servoCount; i++)
{
Console.WriteLine("{0,2}{1,8}{2,8}{3,8}{4,8}", i,
servos[i].target, servos[i].speed,
servos[i].acceleration, servos[i].position);
}
Console.Write("errors: 0x" + variables.errors.ToString("X4"));
foreach (var error in Enum.GetValues(typeof(uscError)))
{
if ((variables.errors & (1 << (int)(uscError)error)) != 0)
{
Console.Write(" " + error.ToString());
}
}
usc.clearErrors();
Console.WriteLine(); // end the line
if (variables.scriptDone == 0)
{
Console.WriteLine("SCRIPT RUNNING");
}
else
{
Console.WriteLine("SCRIPT DONE");
}
Console.WriteLine("program counter: " + variables.programCounter.ToString());
Console.Write("stack: ");
for (int i = 0; i < variables.stackPointer; i++)
{
if (i > stack.Length)
{
Console.Write(" OVERFLOW (stack pointer = " + variables.stackPointer + ")");
break;
}
Console.Write(stack[i].ToString().PadLeft(8, ' '));
}
Console.WriteLine();
Console.Write("call stack: ");
for (int i = 0; i < variables.callStackPointer; i++)
{
if (i > call_stack.Length)
{
Console.Write(" OVERFLOW (call stack pointer = " + variables.callStackPointer + ")");
break;
}
Console.Write(call_stack[i].ToString().PadLeft(8, ' '));
}
Console.WriteLine();
}
/// <summary>
/// Gets the position of a specific servo connected to the maestro
/// </summary>
/// <param name="servoNo">The position of the servo in the maestro</param>
/// <returns>Returns the position</returns>
public int GetServoPosition(int servoNo)
{
// update the servo status first
usc.getVariables(out servos);
return((int)servos[servoNo].position);
}
/// <summary>
///
/// </summary>
/// <param name="usc"></param>
/// <param name="serialNo"></param>
public Maestro(Usc usc, string serialNo)
{
this.serialNumber = serialNo;
this.usc = usc;
usc.getVariables(out servos);
}
