From c7c20184f5f9d9a3eeb1b27ea7250f0f06e4888e Mon Sep 17 00:00:00 2001 From: Amy Blank <ab@barrett.com> Date: Fri, 25 May 2018 12:35:02 -0400 Subject: [PATCH 1/6] added speed and acceleration to point-to-point config file --- .../PointToPointCheckout.cs | 45 +++++++++++++------ .../point_to_point_simulation.json | 4 +- 2 files changed, 35 insertions(+), 14 deletions(-) diff --git a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs index ec50faa1..1f35ca41 100644 --- a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs +++ b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs @@ -1,14 +1,13 @@ -using BurtSharp.CoAP; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.IO; +using System.Threading;using BurtSharp.CoAP; using BurtSharp.CoAP.MsgTypes; using BurtSharp.Control; using BurtSharp.Util.ClassExtensions; using MathNet.Numerics.LinearAlgebra; using Newtonsoft.Json.Linq; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.IO; -using System.Threading; using UnityEngine; namespace BurtSharp.Manufacturing @@ -46,7 +45,10 @@ namespace BurtSharp.Manufacturing // joint position command private Vector<float> jointCommand; private BurtSharp.Control.LinearTrajectoryVector jointTraj; - private const float speed = 0.8f; + private const float kDefaultSpeed = 0.8f; + private const float kDefaultAcceleration = 0.5f; + private float speed = kDefaultSpeed; + private float acceleration = kDefaultAcceleration; // Current repetition int rep = 0; @@ -70,7 +72,7 @@ namespace BurtSharp.Manufacturing EnabledSystems safetySystemsEnabledTemp = new EnabledSystems ( true, // robot_grav_comp false, // user_grav_comp - false, // joint_torque_sat + false, // joint_torque_sat false, // joint_vel_sat false, // cart_vel_sat false, // virtual_joint_stops @@ -283,6 +285,8 @@ namespace BurtSharp.Manufacturing /// Starts movement. /// </summary> public void Start () { + string txt_line; + if (!active) { if (robot.Handedness == RobotHandedness.Right) { home = Vector<float>.Build.DenseOfEnumerable (config ["home"] ["right"].ToObject<List<float>> ()); @@ -290,7 +294,7 @@ namespace BurtSharp.Manufacturing foreach (var c in config ["points"] ["right"]) { points.Add (Vector<float>.Build.DenseOfEnumerable (c.ToObject<List<float>> ())); }; - string txt_line = "Right handed robot detected. Home position " + home.ToVector3 ().ToString (); + txt_line = "Right handed robot detected. Home position " + home.ToVector3 ().ToString (); TryPrintAtPosition (0, line++, txt_line, length); } else if (robot.Handedness == RobotHandedness.Left) { home = Vector<float>.Build.DenseOfEnumerable (config ["home"] ["left"].ToObject<List<float>> ()); @@ -298,13 +302,28 @@ namespace BurtSharp.Manufacturing foreach (var c in config ["points"] ["left"]) { points.Add (Vector<float>.Build.DenseOfEnumerable (c.ToObject<List<float>> ())); }; - string txt_line = "Left handed robot detected. Home position " + home.ToVector3 ().ToString (); + txt_line = "Left handed robot detected. Home position " + home.ToVector3 ().ToString (); TryPrintAtPosition (0, line++, txt_line, length); } else { - string txt_line = "Unknown handedness."; + txt_line = "Unknown handedness."; TryPrintAtPosition (0, line++, txt_line, length); return; } + + if (config ["speed"] != null) { + float temp = config ["speed"].ToObject<float> (); + if (temp > 0) { + speed = temp; + } + } + if (config ["acceleration"] != null) { + float temp = config ["acceleration"].ToObject<float> (); + if (temp > 0) { + acceleration = temp; + } + } + txt_line = string.Format ("Speed {0}, Acceleration {1}", speed.ToString ("F2"), acceleration.ToString ("F2")); + TryPrintAtPosition (0, line++, txt_line, length); SetTemporarySettings (); active = true; @@ -350,9 +369,9 @@ namespace BurtSharp.Manufacturing TryPrintAtPosition (0, line++, txt_line, length); if ((pointIndex == 0) && (rep == 0)) { // first point jointPos.FromVector3 (robot.JointPositions); - jointTraj.BeginMove (jointPos, jointCommand, speed); + jointTraj.BeginMove (jointPos, jointCommand, speed, acceleration); } else { - jointTraj.BeginMove (jointTraj.Position, jointCommand, speed); + jointTraj.BeginMove (jointTraj.Position, jointCommand, speed, acceleration); } } diff --git a/BurtSharp.Manufacturing/PointToPointCheckout/point_to_point_simulation.json b/BurtSharp.Manufacturing/PointToPointCheckout/point_to_point_simulation.json index 2f68c9a1..b81c75ca 100644 --- a/BurtSharp.Manufacturing/PointToPointCheckout/point_to_point_simulation.json +++ b/BurtSharp.Manufacturing/PointToPointCheckout/point_to_point_simulation.json @@ -32,6 +32,8 @@ [ 0.17, 0.52, 1.22 ], [ -0.87, -0.70, 2.62 ] ] - } + }, + "speed": 1.6, + "acceleration": 1.0 } -- GitLab From 98d8c334b7c6802aab038db9f9b3e3f01f51126d Mon Sep 17 00:00:00 2001 From: Amy Blank <ab@barrett.com> Date: Fri, 25 May 2018 18:31:04 -0400 Subject: [PATCH 2/6] added movement and time stats --- .../PointToPointCheckout.cs | 27 +++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs index 1f35ca41..8601dc83 100644 --- a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs +++ b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs @@ -2,7 +2,8 @@ using System.Collections.Generic; using System.Diagnostics; using System.IO; -using System.Threading;using BurtSharp.CoAP; +using System.Threading; +using BurtSharp.CoAP; using BurtSharp.CoAP.MsgTypes; using BurtSharp.Control; using BurtSharp.Util.ClassExtensions; @@ -57,6 +58,10 @@ namespace BurtSharp.Manufacturing List<Vector<float>> points = new List<Vector<float>> (); Vector<float> home; + // Run time stats + private Stopwatch _stopwatch = new Stopwatch (); + private int _totalMovements = 0; + static readonly int kMainLoopTime_ms = 50; // default path for configuration files @@ -132,6 +137,7 @@ namespace BurtSharp.Manufacturing while (isRunning) { if (active) { if (jointTraj.DoneMoving) { + _totalMovements++; if (!done) { pointIndex++; if (pointIndex == points.Count) { @@ -146,6 +152,7 @@ namespace BurtSharp.Manufacturing ClearLine (line, (uint)(Console.WindowHeight - line)); TryPrintAtPosition (0, line++, "Done.", length); active = false; + _stopwatch.Stop (); } } } @@ -168,6 +175,15 @@ namespace BurtSharp.Manufacturing line++; + ClearLine (line, 3); + string txt = "Total number of movements: " + _totalMovements; + PrintAtPosition (0, line++, txt); + float time = (float)_stopwatch.ElapsedMilliseconds / 1000f; + txt = "Total run time: " + time.ToString ("F3") + " s"; + PrintAtPosition (0, line++, txt); + + line++; + ClearLine (line, 14); foreach (string txt_line in debugData.adc.ToString ().Split (Environment.NewLine.ToCharArray (),32)) { PrintAtPosition (0, line++, txt_line); @@ -255,7 +271,7 @@ namespace BurtSharp.Manufacturing { if (active) { Stop (); - } else { + } else if (robot.IsEnabled ()) { Start (); } } @@ -277,6 +293,7 @@ namespace BurtSharp.Manufacturing TryPrintAtPosition (0, line++, "Moving home.", length); home.CopyTo (jointCommand); MoveToJoint (); + done = true; } } @@ -332,6 +349,8 @@ namespace BurtSharp.Manufacturing rep = 0; points [pointIndex].CopyTo (jointCommand); MoveToJoint (); + + _stopwatch.Start (); } } @@ -344,6 +363,9 @@ namespace BurtSharp.Manufacturing RestoreSettings (); active = false; } + if (_stopwatch.IsRunning) { + _stopwatch.Stop (); + } robot.Enable (); } @@ -356,6 +378,7 @@ namespace BurtSharp.Manufacturing RestoreSettings (); active = false; } + _stopwatch.Stop (); robot.Disable (); } -- GitLab From fa193d07ae6f9daa1529eeb36785152364513807 Mon Sep 17 00:00:00 2001 From: Amy Blank <ab@barrett.com> Date: Tue, 29 May 2018 17:51:38 -0400 Subject: [PATCH 3/6] stop timer on fault --- .../PointToPointCheckout.cs | 55 ++++++++++++++++++- 1 file changed, 53 insertions(+), 2 deletions(-) diff --git a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs index 8601dc83..1969cd54 100644 --- a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs +++ b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs @@ -117,6 +117,8 @@ namespace BurtSharp.Manufacturing robot = new RobotControllerSwitchable (RunControlCycle); robot.GetClient ().SubscribeToDebugUpdate (OnReceiveDebugUpdate); robot.PrintDebug = false; + robot.GetClient ().SubscribeToFaults (); + SetupFaultCallbacks (); SetupKeyboardManager (); // Add a blank line for readability, then get the cursor position. @@ -226,7 +228,24 @@ namespace BurtSharp.Manufacturing private void OnReceiveDebugUpdate (DebugUpdate update) { debugData = update; -// Console.WriteLine (debugData.adc.ToString ()); + } + + /// <summary> + /// Stops the checkout when disabling faults are received. + /// </summary> + private void StopOnFault (MainBoardFault code, bool is_set) + { + if (active && is_set) { +// RestoreSettings (); + _stopwatch.Stop (); + active = false; + jointTraj.EndMove (); + jointTorques.Clear (); + jointPid.ResetAll (); + ClearLine (line, (uint)(Console.WindowHeight - line)); + TryPrintAtPosition (0, line++, "Stopping for fault " + code.ToString () + ".", length); + done = true; + } } /// <summary> @@ -240,6 +259,38 @@ namespace BurtSharp.Manufacturing Console.WriteLine ("\n WARNING: Joint torque limits are OFF for this procedure!\n"); } + /// <summary> + /// Register callbacks for faults that should stop the checkout procedure. + /// </summary> + public void SetupFaultCallbacks () + { + robot.GetClient ().RegisterFaultCallback (MainBoardFault.CanHeartbeat, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.CriticalCoapHeartbeat, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.CanMessageTimeout, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.OverVoltage, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.UnderVoltage, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.FuseFailure, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.ElectricalShort, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.EStopPressed, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.EStopLatched, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.OuterLinkDisconnected, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.HandednessUnknown, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.JointEncoderMismatch, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.PuckInError, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.CriticalOverTemperature, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck1CriticalOverTemperature, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck2CriticalOverTemperature, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck3CriticalOverTemperature, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.UserTriggeredSoftHold, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.SafetyWallFault, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.J1EncoderMismatch, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.J2EncoderMismatch, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.J3EncoderMismatch, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck1UnderVoltage, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck2UnderVoltage, StopOnFault); + robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck3UnderVoltage, StopOnFault); + } + /// <summary> /// Sets up the keyboard callbacks and prints the usage menu. /// </summary> @@ -271,7 +322,7 @@ namespace BurtSharp.Manufacturing { if (active) { Stop (); - } else if (robot.IsEnabled ()) { + } else if (robot.IsEnabled () && !robot.GetClient ().GetIsHoldingPosition ()) { Start (); } } -- GitLab From 20fad875a27eb97a38cd987f5b575385062fb57c Mon Sep 17 00:00:00 2001 From: Amy Blank <ab@barrett.com> Date: Wed, 30 May 2018 12:55:01 -0400 Subject: [PATCH 4/6] code review updates --- .../PointToPointCheckout/PointToPointCheckout.cs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs index 1969cd54..d84bf582 100644 --- a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs +++ b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs @@ -48,8 +48,8 @@ namespace BurtSharp.Manufacturing private BurtSharp.Control.LinearTrajectoryVector jointTraj; private const float kDefaultSpeed = 0.8f; private const float kDefaultAcceleration = 0.5f; - private float speed = kDefaultSpeed; - private float acceleration = kDefaultAcceleration; + private float _speed = kDefaultSpeed; + private float _acceleration = kDefaultAcceleration; // Current repetition int rep = 0; @@ -236,7 +236,7 @@ namespace BurtSharp.Manufacturing private void StopOnFault (MainBoardFault code, bool is_set) { if (active && is_set) { -// RestoreSettings (); +// RestoreSettings (); // TODO: This is currently commented out because of bug burt-firmware#262 _stopwatch.Stop (); active = false; jointTraj.EndMove (); @@ -381,16 +381,16 @@ namespace BurtSharp.Manufacturing if (config ["speed"] != null) { float temp = config ["speed"].ToObject<float> (); if (temp > 0) { - speed = temp; + _speed = temp; } } if (config ["acceleration"] != null) { float temp = config ["acceleration"].ToObject<float> (); if (temp > 0) { - acceleration = temp; + _acceleration = temp; } } - txt_line = string.Format ("Speed {0}, Acceleration {1}", speed.ToString ("F2"), acceleration.ToString ("F2")); + txt_line = string.Format ("Speed {0}, Acceleration {1}", _speed.ToString ("F2"), _acceleration.ToString ("F2")); TryPrintAtPosition (0, line++, txt_line, length); SetTemporarySettings (); @@ -443,9 +443,9 @@ namespace BurtSharp.Manufacturing TryPrintAtPosition (0, line++, txt_line, length); if ((pointIndex == 0) && (rep == 0)) { // first point jointPos.FromVector3 (robot.JointPositions); - jointTraj.BeginMove (jointPos, jointCommand, speed, acceleration); + jointTraj.BeginMove (jointPos, jointCommand, _speed, _acceleration); } else { - jointTraj.BeginMove (jointTraj.Position, jointCommand, speed, acceleration); + jointTraj.BeginMove (jointTraj.Position, jointCommand, _speed, _acceleration); } } -- GitLab From 982754857b7710b187a17b6054a39e8843f4016c Mon Sep 17 00:00:00 2001 From: Amy Blank <ab@barrett.com> Date: Wed, 30 May 2018 13:13:05 -0400 Subject: [PATCH 5/6] updates for coding standard --- .../PointToPointCheckout.cs | 344 +++++++++--------- 1 file changed, 172 insertions(+), 172 deletions(-) diff --git a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs index d84bf582..1237da2c 100644 --- a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs +++ b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs @@ -20,61 +20,61 @@ namespace BurtSharp.Manufacturing /// </summary> public class PointToPointCheckoutTest { - private RobotControllerSwitchable robot; - private BurtSharp.KeyboardManager keyboardManager; + private RobotControllerSwitchable _robot; + private BurtSharp.KeyboardManager _keyboardManager; // number of degrees of freedom - const int kDof = 3; + private const int kDof = 3; public static readonly float[] kpJointDefault = { 250, 200, 25 }; public static readonly float[] kiJointDefault = { 0, 0, 0 }; public static readonly float[] kdJointDefault = { 5, 5, 4 }; // current joint positions - private Vector<float> jointPos; + private Vector<float> _jointPos; // current joint velocities - private Vector<float> jointVel; + private Vector<float> _jointVel; // commanded joint torques - private Vector<float> jointTorques; + private Vector<float> _jointTorques; - private BurtSharp.Control.PidVector jointPid; - private Vector<float> kpJoint; - private Vector<float> kiJoint; - private Vector<float> kdJoint; + private BurtSharp.Control.PidVector _jointPid; + private Vector<float> _kpJoint; + private Vector<float> _kiJoint; + private Vector<float> _kdJoint; private bool active = false; // joint position command - private Vector<float> jointCommand; - private BurtSharp.Control.LinearTrajectoryVector jointTraj; + private Vector<float> _jointCommand; + private BurtSharp.Control.LinearTrajectoryVector _jointTraj; private const float kDefaultSpeed = 0.8f; private const float kDefaultAcceleration = 0.5f; private float _speed = kDefaultSpeed; private float _acceleration = kDefaultAcceleration; // Current repetition - int rep = 0; - int pointIndex = 0; - bool done = false; - List<Vector<float>> points = new List<Vector<float>> (); - Vector<float> home; + private int _rep = 0; + private int _pointIndex = 0; + private bool _done = false; + private List<Vector<float>> _points = new List<Vector<float>> (); + private Vector<float> _home; // Run time stats private Stopwatch _stopwatch = new Stopwatch (); private int _totalMovements = 0; - static readonly int kMainLoopTime_ms = 50; + private static readonly int kMainLoopTime_ms = 50; // default path for configuration files - const string kConfFile = "point_to_point_checkout.json"; - JObject config; + private const string kConfFile = "point_to_point_checkout.json"; + private JObject _config; // desired firmware torque saturation limit to run this program - const float kTorqueLimit_Nm = 35.0f; + private const float kTorqueLimit_Nm = 35.0f; // torque limit queried from firmware to restore at the end of this program - List<float> torqueLimitSaved = new List<float> (); + private List<float> _torqueLimitSaved = new List<float> (); // firmware safety systems to leave active during this program - EnabledSystems safetySystemsEnabledTemp = new EnabledSystems ( + private EnabledSystems _safetySystemsEnabledTemp = new EnabledSystems ( true, // robot_grav_comp false, // user_grav_comp false, // joint_torque_sat @@ -85,15 +85,15 @@ namespace BurtSharp.Manufacturing false // safety_cylinder ); // currently active systems queried from firmware to restore at the end of this program - EnabledSystems safetySystemsEnabledSaved = new EnabledSystems (); + private EnabledSystems _safetySystemsEnabledSaved = new EnabledSystems (); // for streaming data to screen - DebugUpdate debugData = new DebugUpdate (); - int top = 0; - int length; - int height; - int line; - readonly string kWindowSizeErrorStr = "-- Error: Expand Window To See Full Text --"; + private DebugUpdate _debugData = new DebugUpdate (); + private int _top = 0; + private int _length; + private int _height; + private int _line; + private readonly string kWindowSizeErrorStr = "-- Error: Expand Window To See Full Text --"; public static void Main (string [] args) { @@ -104,9 +104,9 @@ namespace BurtSharp.Manufacturing { string confFileName = args.Length == 1 ? args [0] : kConfFile; - config = BurtSharp.Util.Paths.LoadJsonConfigFile (confFileName); + _config = BurtSharp.Util.Paths.LoadJsonConfigFile (confFileName); - if (config == null) { + if (_config == null) { BurtSharp.SystemLogger.Error ( "Could not find config file {0}, please try again with a file which exists or is in /opt/barrett/config or ~/.config/barrett", confFileName); @@ -114,45 +114,45 @@ namespace BurtSharp.Manufacturing } InitializeVectors (); - robot = new RobotControllerSwitchable (RunControlCycle); - robot.GetClient ().SubscribeToDebugUpdate (OnReceiveDebugUpdate); - robot.PrintDebug = false; - robot.GetClient ().SubscribeToFaults (); + _robot = new RobotControllerSwitchable (RunControlCycle); + _robot.GetClient ().SubscribeToDebugUpdate (OnReceiveDebugUpdate); + _robot.PrintDebug = false; + _robot.GetClient ().SubscribeToFaults (); SetupFaultCallbacks (); SetupKeyboardManager (); // Add a blank line for readability, then get the cursor position. Console.WriteLine (); - top = Console.CursorTop; - line = top; + _top = Console.CursorTop; + _line = _top; // Move the cursor to a nice place to display other text. - Console.SetCursorPosition (0, line + 4); + Console.SetCursorPosition (0, _line + 4); // Set up PID controller - jointPid = new BurtSharp.Control.PidVector (kpJoint, kiJoint, kdJoint, kDof); + _jointPid = new BurtSharp.Control.PidVector (_kpJoint, _kiJoint, _kdJoint, kDof); // Set up trajectory generator - jointTraj = new BurtSharp.Control.LinearTrajectoryVector (kDof); + _jointTraj = new BurtSharp.Control.LinearTrajectoryVector (kDof); bool isRunning = true; while (isRunning) { if (active) { - if (jointTraj.DoneMoving) { + if (_jointTraj.DoneMoving) { _totalMovements++; - if (!done) { - pointIndex++; - if (pointIndex == points.Count) { - pointIndex = 0; - rep++; - TryPrintAtPosition (0, line++, rep + " repetitions complete.", length); + if (!_done) { + _pointIndex++; + if (_pointIndex == _points.Count) { + _pointIndex = 0; + _rep++; + TryPrintAtPosition (0, _line++, _rep + " repetitions complete.", _length); } - points [pointIndex].CopyTo (jointCommand); + _points [_pointIndex].CopyTo (_jointCommand); MoveToJoint (); } else { - ClearLine (line, (uint)(Console.WindowHeight - line)); - TryPrintAtPosition (0, line++, "Done.", length); + ClearLine (_line, (uint)(Console.WindowHeight - _line)); + TryPrintAtPosition (0, _line++, "Done.", _length); active = false; _stopwatch.Stop (); } @@ -161,40 +161,40 @@ namespace BurtSharp.Manufacturing isRunning = ReadKeyPress (); - if ((height != Console.WindowHeight) || (length != Console.WindowWidth)) { + if ((_height != Console.WindowHeight) || (_length != Console.WindowWidth)) { ResetWindow (); - top = Console.CursorTop; + _top = Console.CursorTop; } try { // Update the display - line = top; + _line = _top; - ClearLine (line, 3); - foreach (string txt_line in robot.Status.ToString ().Split (Environment.NewLine.ToCharArray (),32)) { - PrintAtPosition (0, line++, txt_line); + ClearLine (_line, 3); + foreach (string txt_line in _robot.Status.ToString ().Split (Environment.NewLine.ToCharArray (),32)) { + PrintAtPosition (0, _line++, txt_line); } - line++; + _line++; - ClearLine (line, 3); + ClearLine (_line, 3); string txt = "Total number of movements: " + _totalMovements; - PrintAtPosition (0, line++, txt); + PrintAtPosition (0, _line++, txt); float time = (float)_stopwatch.ElapsedMilliseconds / 1000f; txt = "Total run time: " + time.ToString ("F3") + " s"; - PrintAtPosition (0, line++, txt); + PrintAtPosition (0, _line++, txt); - line++; + _line++; - ClearLine (line, 14); - foreach (string txt_line in debugData.adc.ToString ().Split (Environment.NewLine.ToCharArray (),32)) { - PrintAtPosition (0, line++, txt_line); + ClearLine (_line, 14); + foreach (string txt_line in _debugData.adc.ToString ().Split (Environment.NewLine.ToCharArray (),32)) { + PrintAtPosition (0, _line++, txt_line); } - ClearLine (line++, 2); + ClearLine (_line++, 2); // Move the cursor to a nice place to display user input. - Console.SetCursorPosition (0, line++); + Console.SetCursorPosition (0, _line++); } catch { Console.SetCursorPosition (0, Console.WindowHeight - 1); Console.Write (kWindowSizeErrorStr); @@ -203,23 +203,23 @@ namespace BurtSharp.Manufacturing } Console.WriteLine ("Quitting."); - robot.Dispose (); + _robot.Dispose (); } private RobotCommand RunControlCycle () { - jointPos.FromVector3 (robot.JointPositions); - jointVel.FromVector3 (robot.JointVelocities); + _jointPos.FromVector3 (_robot.JointPositions); + _jointVel.FromVector3 (_robot.JointVelocities); if (active) { - jointTraj.Update (); - jointTorques = jointPid.Update (jointTraj.Position, jointPos, - jointTraj.Velocity, jointVel, robot.TimeSinceLastCommand); + _jointTraj.Update (); + _jointTorques = _jointPid.Update (_jointTraj.Position, _jointPos, + _jointTraj.Velocity, _jointVel, _robot.TimeSinceLastCommand); } else { - jointTorques.Clear (); + _jointTorques.Clear (); } - return new RobotCommand (ControlMode.Torque, jointTorques.ToVector3 ()); + return new RobotCommand (ControlMode.Torque, _jointTorques.ToVector3 ()); } /// <summary> @@ -227,7 +227,7 @@ namespace BurtSharp.Manufacturing /// </summary> private void OnReceiveDebugUpdate (DebugUpdate update) { - debugData = update; + _debugData = update; } /// <summary> @@ -239,12 +239,12 @@ namespace BurtSharp.Manufacturing // RestoreSettings (); // TODO: This is currently commented out because of bug burt-firmware#262 _stopwatch.Stop (); active = false; - jointTraj.EndMove (); - jointTorques.Clear (); - jointPid.ResetAll (); - ClearLine (line, (uint)(Console.WindowHeight - line)); - TryPrintAtPosition (0, line++, "Stopping for fault " + code.ToString () + ".", length); - done = true; + _jointTraj.EndMove (); + _jointTorques.Clear (); + _jointPid.ResetAll (); + ClearLine (_line, (uint)(Console.WindowHeight - _line)); + TryPrintAtPosition (0, _line++, "Stopping for fault " + code.ToString () + ".", _length); + _done = true; } } @@ -264,31 +264,31 @@ namespace BurtSharp.Manufacturing /// </summary> public void SetupFaultCallbacks () { - robot.GetClient ().RegisterFaultCallback (MainBoardFault.CanHeartbeat, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.CriticalCoapHeartbeat, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.CanMessageTimeout, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.OverVoltage, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.UnderVoltage, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.FuseFailure, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.ElectricalShort, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.EStopPressed, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.EStopLatched, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.OuterLinkDisconnected, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.HandednessUnknown, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.JointEncoderMismatch, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.PuckInError, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.CriticalOverTemperature, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck1CriticalOverTemperature, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck2CriticalOverTemperature, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck3CriticalOverTemperature, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.UserTriggeredSoftHold, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.SafetyWallFault, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.J1EncoderMismatch, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.J2EncoderMismatch, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.J3EncoderMismatch, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck1UnderVoltage, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck2UnderVoltage, StopOnFault); - robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck3UnderVoltage, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.CanHeartbeat, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.CriticalCoapHeartbeat, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.CanMessageTimeout, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.OverVoltage, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.UnderVoltage, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.FuseFailure, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.ElectricalShort, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.EStopPressed, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.EStopLatched, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.OuterLinkDisconnected, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.HandednessUnknown, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.JointEncoderMismatch, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.PuckInError, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.CriticalOverTemperature, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck1CriticalOverTemperature, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck2CriticalOverTemperature, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck3CriticalOverTemperature, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.UserTriggeredSoftHold, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.SafetyWallFault, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.J1EncoderMismatch, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.J2EncoderMismatch, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.J3EncoderMismatch, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck1UnderVoltage, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck2UnderVoltage, StopOnFault); + _robot.GetClient ().RegisterFaultCallback (MainBoardFault.Puck3UnderVoltage, StopOnFault); } /// <summary> @@ -296,10 +296,10 @@ namespace BurtSharp.Manufacturing /// </summary> public void SetupKeyboardManager () { - keyboardManager = new BurtSharp.KeyboardManager (); - keyboardManager.AddKeyPressCallback ("e", OnEnable); - keyboardManager.AddKeyPressCallback ("d", OnDisable); - keyboardManager.AddKeyPressCallback ("s", StartStop); + _keyboardManager = new BurtSharp.KeyboardManager (); + _keyboardManager.AddKeyPressCallback ("e", OnEnable); + _keyboardManager.AddKeyPressCallback ("d", OnDisable); + _keyboardManager.AddKeyPressCallback ("s", StartStop); ResetWindow (); } @@ -309,8 +309,8 @@ namespace BurtSharp.Manufacturing public void ResetWindow () { Console.Clear (); - length = Console.WindowWidth; - height = Console.WindowHeight; + _length = Console.WindowWidth; + _height = Console.WindowHeight; Console.SetCursorPosition (0, 0); PrintUsage (); } @@ -322,7 +322,7 @@ namespace BurtSharp.Manufacturing { if (active) { Stop (); - } else if (robot.IsEnabled () && !robot.GetClient ().GetIsHoldingPosition ()) { + } else if (_robot.IsEnabled () && !_robot.GetClient ().GetIsHoldingPosition ()) { Start (); } } @@ -332,20 +332,20 @@ namespace BurtSharp.Manufacturing /// </summary> public void Stop () { - ClearLine (line, (uint)(Console.WindowHeight - line)); + ClearLine (_line, (uint)(Console.WindowHeight - _line)); if (active) { RestoreSettings (); - jointTraj.EndMove (); - jointTorques.Clear (); - jointPid.ResetAll (); + _jointTraj.EndMove (); + _jointTorques.Clear (); + _jointPid.ResetAll (); - ClearLine (line, (uint)(Console.WindowHeight - line)); - TryPrintAtPosition (0, line++, "Moving home.", length); - home.CopyTo (jointCommand); + ClearLine (_line, (uint)(Console.WindowHeight - _line)); + TryPrintAtPosition (0, _line++, "Moving home.", _length); + _home.CopyTo (_jointCommand); MoveToJoint (); - done = true; + _done = true; } } @@ -356,49 +356,49 @@ namespace BurtSharp.Manufacturing string txt_line; if (!active) { - if (robot.Handedness == RobotHandedness.Right) { - home = Vector<float>.Build.DenseOfEnumerable (config ["home"] ["right"].ToObject<List<float>> ()); - points.Clear (); - foreach (var c in config ["points"] ["right"]) { - points.Add (Vector<float>.Build.DenseOfEnumerable (c.ToObject<List<float>> ())); + if (_robot.Handedness == RobotHandedness.Right) { + _home = Vector<float>.Build.DenseOfEnumerable (_config ["home"] ["right"].ToObject<List<float>> ()); + _points.Clear (); + foreach (var c in _config ["points"] ["right"]) { + _points.Add (Vector<float>.Build.DenseOfEnumerable (c.ToObject<List<float>> ())); }; - txt_line = "Right handed robot detected. Home position " + home.ToVector3 ().ToString (); - TryPrintAtPosition (0, line++, txt_line, length); - } else if (robot.Handedness == RobotHandedness.Left) { - home = Vector<float>.Build.DenseOfEnumerable (config ["home"] ["left"].ToObject<List<float>> ()); - points.Clear (); - foreach (var c in config ["points"] ["left"]) { - points.Add (Vector<float>.Build.DenseOfEnumerable (c.ToObject<List<float>> ())); + txt_line = "Right handed robot detected. Home position " + _home.ToVector3 ().ToString (); + TryPrintAtPosition (0, _line++, txt_line, _length); + } else if (_robot.Handedness == RobotHandedness.Left) { + _home = Vector<float>.Build.DenseOfEnumerable (_config ["home"] ["left"].ToObject<List<float>> ()); + _points.Clear (); + foreach (var c in _config ["points"] ["left"]) { + _points.Add (Vector<float>.Build.DenseOfEnumerable (c.ToObject<List<float>> ())); }; - txt_line = "Left handed robot detected. Home position " + home.ToVector3 ().ToString (); - TryPrintAtPosition (0, line++, txt_line, length); + txt_line = "Left handed robot detected. Home position " + _home.ToVector3 ().ToString (); + TryPrintAtPosition (0, _line++, txt_line, _length); } else { txt_line = "Unknown handedness."; - TryPrintAtPosition (0, line++, txt_line, length); + TryPrintAtPosition (0, _line++, txt_line, _length); return; } - if (config ["speed"] != null) { - float temp = config ["speed"].ToObject<float> (); + if (_config ["speed"] != null) { + float temp = _config ["speed"].ToObject<float> (); if (temp > 0) { _speed = temp; } } - if (config ["acceleration"] != null) { - float temp = config ["acceleration"].ToObject<float> (); + if (_config ["acceleration"] != null) { + float temp = _config ["acceleration"].ToObject<float> (); if (temp > 0) { _acceleration = temp; } } txt_line = string.Format ("Speed {0}, Acceleration {1}", _speed.ToString ("F2"), _acceleration.ToString ("F2")); - TryPrintAtPosition (0, line++, txt_line, length); + TryPrintAtPosition (0, _line++, txt_line, _length); SetTemporarySettings (); active = true; - done = false; - pointIndex = 0; - rep = 0; - points [pointIndex].CopyTo (jointCommand); + _done = false; + _pointIndex = 0; + _rep = 0; + _points [_pointIndex].CopyTo (_jointCommand); MoveToJoint (); _stopwatch.Start (); @@ -417,7 +417,7 @@ namespace BurtSharp.Manufacturing if (_stopwatch.IsRunning) { _stopwatch.Stop (); } - robot.Enable (); + _robot.Enable (); } /// <summary> @@ -430,7 +430,7 @@ namespace BurtSharp.Manufacturing active = false; } _stopwatch.Stop (); - robot.Disable (); + _robot.Disable (); } /// <summary> @@ -438,14 +438,14 @@ namespace BurtSharp.Manufacturing /// </summary> public void MoveToJoint () { - ClearLine (line, (uint)(Console.WindowHeight - line)); - string txt_line = "Moving to joint position " + jointCommand.ToVector3 ().ToString (); - TryPrintAtPosition (0, line++, txt_line, length); - if ((pointIndex == 0) && (rep == 0)) { // first point - jointPos.FromVector3 (robot.JointPositions); - jointTraj.BeginMove (jointPos, jointCommand, _speed, _acceleration); + ClearLine (_line, (uint)(Console.WindowHeight - _line)); + string txt_line = "Moving to joint position " + _jointCommand.ToVector3 ().ToString (); + TryPrintAtPosition (0, _line++, txt_line, _length); + if ((_pointIndex == 0) && (_rep == 0)) { // first point + _jointPos.FromVector3 (_robot.JointPositions); + _jointTraj.BeginMove (_jointPos, _jointCommand, _speed, _acceleration); } else { - jointTraj.BeginMove (jointTraj.Position, jointCommand, _speed, _acceleration); + _jointTraj.BeginMove (_jointTraj.Position, _jointCommand, _speed, _acceleration); } } @@ -509,10 +509,10 @@ namespace BurtSharp.Manufacturing // Handle the new command string keyPressed = Console.ReadKey (false).KeyChar.ToString (); - keyboardManager.HandleKeyPress (keyPressed); + _keyboardManager.HandleKeyPress (keyPressed); if (keyPressed.Equals ("q") || keyPressed.Equals ("Q")) { ClearLine (line, (uint)(Console.WindowHeight - line)); - TryPrintAtPosition (0, line++, "Quitting.", length); + TryPrintAtPosition (0, line++, "Quitting.", _length); return false; } } @@ -521,30 +521,30 @@ namespace BurtSharp.Manufacturing void InitializeVectors () { - jointPos = Vector<float>.Build.Dense (kDof); - jointVel = Vector<float>.Build.Dense (kDof); - jointTorques = Vector<float>.Build.Dense (kDof); - jointCommand = Vector<float>.Build.Dense (kDof); - kpJoint = Vector<float>.Build.DenseOfArray (kpJointDefault); - kiJoint = Vector<float>.Build.DenseOfArray (kiJointDefault); - kdJoint = Vector<float>.Build.DenseOfArray(kdJointDefault); - home = Vector<float>.Build.Dense (kDof); + _jointPos = Vector<float>.Build.Dense (kDof); + _jointVel = Vector<float>.Build.Dense (kDof); + _jointTorques = Vector<float>.Build.Dense (kDof); + _jointCommand = Vector<float>.Build.Dense (kDof); + _kpJoint = Vector<float>.Build.DenseOfArray (kpJointDefault); + _kiJoint = Vector<float>.Build.DenseOfArray (kiJointDefault); + _kdJoint = Vector<float>.Build.DenseOfArray(kdJointDefault); + _home = Vector<float>.Build.Dense (kDof); } void SetTemporarySettings () { - RobotClient client = robot.GetClient (); - torqueLimitSaved = client.GetJointTorqueLimits (); + RobotClient client = _robot.GetClient (); + _torqueLimitSaved = client.GetJointTorqueLimits (); client.SendJointTorqueLimits (new List<float> { kTorqueLimit_Nm, kTorqueLimit_Nm, kTorqueLimit_Nm }); - safetySystemsEnabledSaved = client.GetEnabledSystems (); - client.SendEnabledSystems (safetySystemsEnabledTemp); + _safetySystemsEnabledSaved = client.GetEnabledSystems (); + client.SendEnabledSystems (_safetySystemsEnabledTemp); } void RestoreSettings () { - RobotClient client = robot.GetClient (); - client.SendJointTorqueLimits (torqueLimitSaved); - client.SendEnabledSystems (safetySystemsEnabledSaved); + RobotClient client = _robot.GetClient (); + client.SendJointTorqueLimits (_torqueLimitSaved); + client.SendEnabledSystems (_safetySystemsEnabledSaved); } } } -- GitLab From 4535383c96d10ef707d2c2f172b1e735e08edeea Mon Sep 17 00:00:00 2001 From: Amy Blank <ab@barrett.com> Date: Wed, 30 May 2018 13:18:59 -0400 Subject: [PATCH 6/6] error messages when trying to run in fault state --- .../PointToPointCheckout/PointToPointCheckout.cs | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs index 1237da2c..7f8443ed 100644 --- a/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs +++ b/BurtSharp.Manufacturing/PointToPointCheckout/PointToPointCheckout.cs @@ -322,7 +322,13 @@ namespace BurtSharp.Manufacturing { if (active) { Stop (); - } else if (_robot.IsEnabled () && !_robot.GetClient ().GetIsHoldingPosition ()) { + } else if (!_robot.IsEnabled ()) { + ClearLine (_line, (uint)(Console.WindowHeight - _line)); + TryPrintAtPosition (0, _line++, "Cannot start until robot is enabled. Resolve any fault conditions and press 'e' to enable.", _length); + } else if (_robot.GetClient ().GetIsHoldingPosition ()) { + ClearLine (_line, (uint)(Console.WindowHeight - _line)); + TryPrintAtPosition (0, _line++, "Cannot start with robot in soft hold. Resolve any fault conditions and press 'e' to enable.", _length); + } else { Start (); } } -- GitLab