/// <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); }