public override bool Connect()
{
try
{
if (RobotClient == null)
{
RobotClientSetup();
}
if (ZarizeniJeDlePinguOnline)
{
RobotClient?.Connect();
}
bool status = false;
status = RobotClient.Connected;
if (status)
{
Status = Stav.Online;
NastaveniHlidacihoPsa();
HlidaciPes.Zapni();
}
return(status);
}
catch (Exception exception)
{
var zarizeniArgs = new ZarizeniArgs(Status, Ping, $"!CHYBA!", exception);
OnStatusChanged(zarizeniArgs);
return(false);
}
}
private async void OnLoaded(object sender, RoutedEventArgs e)
{
try
{
var client = new RobotClient();
var processes = await client.GetProcesses();
foreach (var process in processes.OrderBy(p => p.Name))
{
processNameListBox.Items.Add(process.Name);
}
if (processNameListBox.Items.Count > 0)
{
processNameListBox.IsEnabled = true;
if (SelectedProcessName != null)
{
var selected = SelectedProcessName;
SelectedProcessName = null;
foreach (string name in processNameListBox.Items)
{
if (name == selected)
{
processNameListBox.SelectedValue = SelectedProcessName = name;
okButton.IsEnabled = true;
break;
}
}
}
}
}
catch (Exception)
{
MessageBox.Show(this, "Unable to get process names.", "Error", MessageBoxButton.OK, MessageBoxImage.Error);
}
}
static void Main(string[] args)
{
var client = new RobotClient(new SignalRAdapter());
client.ServiceProxy.Value.SendMessage("ohhhhhhhhhhhh");
Console.Read();
}
/// <summary>
/// Initializes a new instance of the <see cref="BaseComm"/> class.
/// </summary>
public BaseCommExample()
{
// Set up communication with the robot and initialize force to zero
robot = new RobotClient();
var version = robot.GetVersion();
Barrett.Logger.Debug(Barrett.Logger.INFO, version.ToString());
robot.SendCartesianForces(Vector3.zero);
// Set up keyboard callbacks. PrintUsage needs to be updated when these are changed.
keyboardManager = new Barrett.KeyboardManager();
keyboardManager.SetDebug(true); // print key pressed
keyboardManager.AddKeyPressCallback("q", Close); // this will call the Close method when 'q' is pressed
keyboardManager.AddKeyPressCallback("h", OnHome);
keyboardManager.AddKeyPressCallback("e", OnEnable);
keyboardManager.AddKeyPressCallback("d", OnDisable);
keyboardManager.AddKeyPressCallback("t", SubscribeToUpdate);
PrintUsage();
// Loop: send zero force at every timestep.
bool running = true;
while (running)
{
running = ReadKeyPress();
Thread.Sleep(50);
robot.SendCartesianForces(Vector3.zero);
}
}
static void Main(string[] args)
{
RobotClient.Connect(new Program(), "127.0.0.1", 60604, 1361778219);
while (true)
{
string msg = Console.ReadLine();
RobotClient.SendMessage(msg);
}
}
public AgvService()
{
_jsonDatabaseService = FreshIOC.Container.Resolve <IJsonDatabaseService>();
_barmanService = FreshIOC.Container.Resolve <IBarmanService>();
_agv = _jsonDatabaseService.GetAgvData();
var robotData = _jsonDatabaseService.GetAgvData();
_robot = new RobotClient("Miron", IPAddress.Parse(robotData.Ip), robotData.Port);
}
static LogicMapper()
{
//Use this class to set which classes the program should use in processing
//Simply change any of the objects below to read = new YOURCLASSHERE();
MotorSubFrameProcessor = new SchiavoneMotorSubProcessor();
ServoSubFrameProcessor = new NullServoSubProcessor();
RobotClient = new RobotClient <T>();
InputProcessor = new SchiavoneInputProcessor();
DashboardModifier = new DefaultDashboardModifier <T>();
}
protected override void PingniNaZarizeni()
{
base.PingniNaZarizeni();
if (RobotClient != null)
{
if (RobotClient.Connected == false)
{
RobotClient.Connect();
}
}
}
public static void RunServerWithClientUpdater(IAppConfigBiz appConfigBiz, IFeedBusiness feedBusiness, IFeedItemBusiness feedItemBusiness, IUpdaterDurationBusiness updaterDurationBusiness)
{
Mn.NewsCms.Common.EventsLog.GeneralLogs.WriteLogInDB("RunServerWithClientUpdater", TypeOfLog.Start);
var baseserver = new BaseServer(appConfigBiz, feedBusiness, feedItemBusiness, updaterDurationBusiness);
var Clientupdater = new ClientUpdater(baseserver, feedBusiness, appConfigBiz, true);
IRobotClient <BaseUpdaterClient> client = new RobotClient <BaseUpdaterClient>()
{
EndPoint = Clientupdater
};
var server = new UpdaterServer <BaseUpdaterClient>(client, true);
server.UpdateIsParting();
Clientupdater.AutoUpdateFromServer();
}
private IReadOnlyDictionary <string, object> RunProcess(string processName, IDictionary <string, object> inputValues)
{
var client = new RobotClient();
var task1 = client.GetProcesses();
task1.Wait();
var processes = task1.Result;
var myProcess = processes.Single(process => process.Name == processName);
var job = myProcess.ToJob();
job.InputArguments = inputValues;
var task2 = client.RunJob(job);
task2.Wait();
return(task2.Result.Arguments);
}
private void RobotClientSetup()
{
PingniNaZarizeni();
RobotClient = new RobotClient(Hostname, IPAddress.Parse(IpAdress), Port)
{
ManageJobs = ArclBase.JobManagement.Full,
QueuedResponses = ArclBase.ResponseFlag.All,
QueueAll = true,
Password = "******",
ResponseTimeout = 1000,
AutoReconnect = true
};
RobotClient.Responses.MaxLength = 200;
RobotClient.Responses.QueueChanged += Responses_QueueChanged;
RobotClient.NewJobQueued += RobotClient_NewJobQueued;
}
/// <summary>
/// Runs the designated process asynchronously.
/// </summary>
/// <param name="processName">Name of the process to run</param>
/// <returns>Task with JobOutput</returns>
private static async Task <JobOutput> RunAsync(string processName)
{
try
{
var client = new RobotClient();
var processes = await client.GetProcesses();
var myProcess = processes.Single(process => process.Name == processName);
var job = myProcess.ToJob();
return(await client.RunJob(job));
}
catch (Exception ex)
{
Console.Error.WriteLine("{0}", ex.ToString());
return(null);
}
}
public override bool Disconnect()
{
try
{
RobotClient?.Disconnect();
bool status = false;
status = (bool)RobotClient?.Connected ? status : false;
Status = Stav.Offline;
return(true);
}
catch (Exception exception)
{
Status = Stav.InError;
var zarizeniArgs = new ZarizeniArgs(Status, Ping, $"!CHYBA!", exception);
OnStatusChanged(zarizeniArgs);
return(false);
}
}
/// <summary>
/// Sends an outbound socket message containing script data to be executed
/// </summary>
/// <param name="machineName"></param>
/// <param name="scriptName"></param>
/// <returns></returns>
public static string SendScriptToClient(string machineName, string scriptName)
{
List <RobotClient> connectedClients = WorkForceManagement.GetClients();
RobotClient requiredConn = connectedClients.Where(client => client.MachineName == machineName).FirstOrDefault();
if (requiredConn != null)
{
var rpaScriptsFolder = Environment.GetFolderPath(Environment.SpecialFolder.MyDocuments) + "\\sharpRPA\\My Scripts\\";
System.Xml.XmlDocument dom = new System.Xml.XmlDocument();
dom.Load(rpaScriptsFolder + scriptName);
byte[] bytes = System.Text.Encoding.Default.GetBytes(dom.OuterXml);
var buffer = new ArraySegment <Byte>(bytes, 0, bytes.Length);
requiredConn.ClientSocket.SendAsync(buffer, WebSocketMessageType.Text, true, CancellationToken.None);
return("Script Sent To Client");
}
else
{
return("Client Not Found");
}
}
public static void RunUpdaterServer()
{
#region RemoteUpdater
var remoteUpdater = RobotClient <string> .DefaultDistributeHostUrl;
if (!string.IsNullOrEmpty(remoteUpdater) && !string.IsNullOrEmpty(Utility.GetWebText(remoteUpdater[remoteUpdater.Length - 1] == '/' ? remoteUpdater.Remove(remoteUpdater.Length - 1) : remoteUpdater)))
{
GeneralLogs.WriteLogInDB("GetWebText from " + remoteUpdater, TypeOfLog.Info);
IRobotClient <string> client = new RobotClient <string>()
{
EndPoint = remoteUpdater
};
var server = new UpdaterServer <string>(client, null);
server.UpdateIsParting();
}
else
{
GeneralLogs.WriteLogInDB("Can not GetWebText from " + remoteUpdater, TypeOfLog.Error);
}
#endregion
}
public SpringExample()
{
// Initialize vectors. This is required to set the length of the vectors before they
// are used. Build.Dense initializes all the elements to 0. Build.DenseOfArray sets
// the vector length to the length of the array and the values equal to the values
// in the array.
jointPos = Vector <float> .Build.Dense(kDof);
jointSpringPos = Vector <float> .Build.Dense(kDof);
jointTorques = Vector <float> .Build.Dense(kDof);
toolPos = Vector <float> .Build.Dense(kNumDim);
cartSpringPos = Vector <float> .Build.Dense(kNumDim);
toolForce = Vector <float> .Build.Dense(kNumDim);
kpJoint = Vector <float> .Build.DenseOfArray(kpJointDefault);
// Set up communication with the robot.
robot = new RobotClient();
robot.SubscribeToServerUpdate(OnReceiveServerUpdate);
robot.SubscribeToRobotStatus(status => {
Barrett.Logger.Debug(Barrett.Logger.INFO, "Handedness: {0}", status.handedness);
Barrett.Logger.Debug(Barrett.Logger.INFO, "Outerlink: {0}", status.outerlink);
Barrett.Logger.Debug(Barrett.Logger.INFO, "IsPatientConnected?: {0}", status.patient);
});
// Initialize force/torque inputs to zero. Here, both are initialized separately. Alternately,
// both can be set in a single command with
// robot.SendCartesianForcesAndJointTorques (Vector3.zero, Vector3.zero);
robot.SendCartesianForces(Vector3.zero);
robot.SendJointTorques(Vector3.zero);
// Set up keyboard callbacks
keyboardManager = new Barrett.KeyboardManager();
keyboardManager.SetDebug(true); // print key pressed
keyboardManager.AddKeyPressCallback("h", OnHome);
keyboardManager.AddKeyPressCallback("e", OnEnable);
keyboardManager.AddKeyPressCallback("d", OnDisable);
keyboardManager.AddKeyPressCallback("1", JointSpring);
keyboardManager.AddKeyPressCallback("2", JointSpring);
keyboardManager.AddKeyPressCallback("3", JointSpring);
keyboardManager.AddKeyPressCallback("x", CartesianSpring);
keyboardManager.AddKeyPressCallback("y", CartesianSpring);
keyboardManager.AddKeyPressCallback("z", CartesianSpring);
keyboardManager.AddKeyPressCallback("r", RemoveAllSprings);
keyboardManager.AddKeyPressCallback("p", PrintInfo);
keyboardManager.AddKeyPressCallback("q", Close);
PrintUsage();
// Loop: calculate forces/torques at every timestep based on current
// state feedback from the robot.
bool running = true;
intervalTimer.Reset();
while (running)
{
running = ReadKeyPress();
jointTorques.Clear();
toolForce.Clear();
jointTorques = CalcJointSpringTorques();
toolForce = CalcCartesianSpringForces();
robot.SendCartesianForcesAndJointTorques(toolForce.ToVector3(), jointTorques.ToVector3())
.Catch(e => Barrett.Logger.Debug(Barrett.Logger.CRITICAL, "Exception {0}", e))
.Done();
Thread.Sleep(Math.Max(0, controlLoopTime - (int)intervalTimer.ElapsedMilliseconds));
intervalTimer.Restart();
}
}
public SimpleMoveExample()
{
// Initialize vectors
jointPos = Vector <float> .Build.Dense(kDof);
jointTorques = Vector <float> .Build.Dense(kDof);
toolPos = Vector <float> .Build.Dense(kNumDim);
toolForce = Vector <float> .Build.Dense(kNumDim);
jointCommand = Vector <float> .Build.Dense(kDof);
toolCommand = Vector <float> .Build.Dense(kNumDim);
kpJoint = Vector <float> .Build.DenseOfArray(kpJointDefault);
kiJoint = Vector <float> .Build.DenseOfArray(kiJointDefault);
kdJoint = Vector <float> .Build.DenseOfArray(kdJointDefault);
// Set up communication with the robot and initialize force/torque to zero
robot = new RobotClient();
robot.SubscribeToServerUpdate(OnReceiveServerUpdate);
robot.SubscribeToRobotStatus(status => {
Barrett.Logger.Debug(Barrett.Logger.INFO, "Handedness: {0}", status.handedness);
Barrett.Logger.Debug(Barrett.Logger.INFO, "Outerlink: {0}", status.outerlink);
Barrett.Logger.Debug(Barrett.Logger.INFO, "IsPatientConnected?: {0}", status.patient);
});
robot.SendCartesianForces(Vector3.zero);
robot.SendJointTorques(Vector3.zero);
// Set up keyboard callbacks
keyboardManager = new Barrett.KeyboardManager();
keyboardManager.SetDebug(true); // print key pressed
keyboardManager.AddKeyPressCallback("h", OnHome);
keyboardManager.AddKeyPressCallback("e", OnEnable);
keyboardManager.AddKeyPressCallback("d", OnDisable);
keyboardManager.AddKeyPressCallback("j", MoveToJoint);
keyboardManager.AddKeyPressCallback("t", MoveToTool);
keyboardManager.AddKeyPressCallback("i", OnIdle);
keyboardManager.AddKeyPressCallback("p", PrintInfo);
keyboardManager.AddKeyPressCallback("q", Close);
PrintUsage();
// Set up PID controllers
jointPid = new Barrett.Control.PidVector(kpJoint, kiJoint, kdJoint, kDof, lowpassFilterFreq);
toolPid = new Barrett.Control.PidVector(kpTool, kiTool, kdTool, kNumDim, lowpassFilterFreq);
// Set up trajectory generators
jointTraj = new Barrett.Control.LinearTrajectoryVector(kDof);
toolTraj = new Barrett.Control.LinearTrajectoryVector(kNumDim);
// Start the dtTimer
dtTimer.Reset();
dtTimer.Start();
// Loop: calculate forces/torques at every timestep based on current
// state feedback from the robot.
bool running = true;
intervalTimer.Reset();
while (running)
{
running = ReadKeyPress();
float dt = (float)dtTimer.ElapsedTicks / (float)Stopwatch.Frequency;
dtTimer.Restart();
if (jointActive)
{
jointTraj.Update();
jointTorques = jointPid.Update(jointTraj.Position, jointPos, dt);
toolForce.Clear();
}
else if (toolActive)
{
toolTraj.Update();
toolForce = toolPid.Update(toolTraj.Position, toolPos, dt);
jointTorques.Clear();
}
else
{
jointTorques.Clear();
toolForce.Clear();
}
robot.SendCartesianForcesAndJointTorques(toolForce.ToVector3(), jointTorques.ToVector3())
.Catch(e => Barrett.Logger.Debug(Barrett.Logger.CRITICAL, "Exception {0}", e))
.Done();
// Calculate how long to wait until next control cycle
Thread.Sleep(Math.Max(0, controlLoopTime - (int)intervalTimer.ElapsedMilliseconds));
intervalTimer.Restart();
}
}
public override void OnEvent(EventData photonEvent)
{
switch (photonEvent.Code)
{
case EventCode.AppStats:
break;
case EventCode.LobbyStats:
this.RoomInfoList.Keys.ToList().ForEach(roomName =>
{
var roomProp = this.RoomInfoList[roomName] as RoomInfo;
var playerCount = roomProp.PlayerCount;
var maxPlayers = roomProp.MaxPlayers;
var isOpen = roomProp.IsOpen;
if (isOpen && playerCount < maxPlayers)
{
RoomShared roomShared = new RoomShared();
for (int i = playerCount, clientId = 0; i < maxPlayers; ++i, ++clientId)
{
RobotClient client = new RobotClient(true, clientId, roomShared);
client.OpJoinRoom(roomName);
}
}
});
break;
case EventCode.GameListUpdate:
// todo: Enter a room automatically.
if (photonEvent.Parameters[ParameterCode.GameList] is Hashtable)
{
var hashtable = photonEvent.Parameters[ParameterCode.GameList] as Hashtable;
this.RoomInfoList.Keys.ToList().ForEach(roomName =>
{
var roomNames = hashtable.Keys.OfType <string>();
if (roomNames.Count() > 0 && roomNames.Contains(roomName))
{
var roomProp = hashtable[roomName] as Hashtable;
var playerCount = Convert.ToInt32(roomProp[GamePropertyKey.PlayerCount]);
var maxPlayers = Convert.ToInt32(roomProp[GamePropertyKey.MaxPlayers]);
var isOpen = Convert.ToBoolean(roomProp[GamePropertyKey.IsOpen]);
if (isOpen && playerCount < maxPlayers)
{
RoomShared roomShared = new RoomShared();
for (int i = playerCount, clientId = 0; i < maxPlayers; ++i, ++clientId)
{
RobotClient client = new RobotClient(true, clientId, roomShared);
client.OnStateChangeAction += (ClientState obj) =>
{
if (obj == ClientState.JoinedLobby)
{
client.OpJoinRoom(roomName);
}
};
}
}
}
});
}
break;
}
base.OnEvent(photonEvent);
}
/// <summary>
/// Initializes a new instance of the <see cref="DisplayBasicInfo"/> class.
/// </summary>
public DisplayBasicInfo()
{
// Set up communication with the robot and initialize force to zero.
robot = new RobotClient();
SubscribeToUpdates();
robot.SendCartesianForces(Vector3.zero);
// Set up keyboard callbacks.
keyboardManager = new Barrett.KeyboardManager();
keyboardManager.SetDebug(true); // print key pressed
keyboardManager.AddKeyPressCallback("q", Close);
keyboardManager.AddKeyPressCallback("h", OnHome);
keyboardManager.AddKeyPressCallback("e", OnEnable);
keyboardManager.AddKeyPressCallback("d", OnDisable);
PrintUsage();
// Add some blank lines for readability, then get the cursor position.
Console.WriteLine();
Console.WriteLine();
int top = Console.CursorTop;
// Print labels, which do not change. Add an offset on the left side for readability
// and specify the length of the strings to clear any text that already exists in
// that space. Make sure that the length is longer than your strings to leave enough
// space.
int line = top;
int left = 10;
int length = 20;
PrintAtPosition(left, line++, "Joint positions:", length);
PrintAtPosition(left, line++, "Tool position:", length);
PrintAtPosition(left, line++, "Tool Velocity:", length);
PrintAtPosition(left, line++, "Handedness:", length);
PrintAtPosition(left, line++, "Outer link status:", length);
// Set the start position on each line for the data.
left += length;
length = 30;
// Loop: Display most recent state info, check for new key presses, and send zero force.
bool running = true;
while (running)
{
line = top;
PrintAtPosition(left, line++, joint_position.ToString("F4"), length);
PrintAtPosition(left, line++, tool_position.ToString("F4"), length);
PrintAtPosition(left, line++, tool_velocity.ToString("F4"), length);
PrintAtPosition(left, line++, handedness.ToString(), length);
PrintAtPosition(left, line++, outerlinkStatus.ToString(), length);
// Move the cursor to a nice place to display user input.
line += 2;
Console.SetCursorPosition(0, line);
running = ReadKeyPress();
Thread.Sleep(50);
robot.SendCartesianForces(Vector3.zero);
}
Console.Write("Quitting.");
Environment.Exit(0);
}
public override void ProvedPrikaz(RobotPrikaz prikaz)
{
RobotClient?.SendCommand(prikaz.ToString());
}
public HoldPositionExample()
{
// Initialize vectors. This is required to set the length of the vectors before they
// are used. Build.Dense initializes all the elements to 0. Build.DenseOfArray sets
// the vector length to the length of the array and the values equal to the values
// in the array.
jointPos = Vector <float> .Build.Dense(kDof);
jointHoldPos = Vector <float> .Build.Dense(kDof);
jointTorques = Vector <float> .Build.Dense(kDof);
toolPos = Vector <float> .Build.Dense(kNumDim);
toolHoldPos = Vector <float> .Build.Dense(kNumDim);
toolForce = Vector <float> .Build.Dense(kNumDim);
kpJoint = Vector <float> .Build.DenseOfArray(kpJointDefault);
kiJoint = Vector <float> .Build.DenseOfArray(kiJointDefault);
kdJoint = Vector <float> .Build.DenseOfArray(kdJointDefault);
// Set up communication with the robot.
robot = new RobotClient();
robot.SubscribeToServerUpdate(OnReceiveServerUpdate);
robot.SubscribeToRobotStatus(status => {
Barrett.Logger.Debug(Barrett.Logger.INFO, "Handedness: {0}", status.handedness);
Barrett.Logger.Debug(Barrett.Logger.INFO, "Outerlink: {0}", status.outerlink);
Barrett.Logger.Debug(Barrett.Logger.INFO, "IsPatientConnected?: {0}", status.patient);
});
// Initialize force/torque inputs to zero. Here, both are initialized separately. Alternately,
// both can be set in a single command with
// robot.SendCartesianForcesAndJointTorques (Vector3.zero, Vector3.zero);
robot.SendCartesianForces(Vector3.zero);
robot.SendJointTorques(Vector3.zero);
// Set up keyboard callbacks
keyboardManager = new Barrett.KeyboardManager();
keyboardManager.SetDebug(true); // print key pressed
keyboardManager.AddKeyPressCallback("h", OnHome);
keyboardManager.AddKeyPressCallback("e", OnEnable);
keyboardManager.AddKeyPressCallback("d", OnDisable);
keyboardManager.AddKeyPressCallback("j", HoldJointPosition);
keyboardManager.AddKeyPressCallback("t", HoldToolPosition);
keyboardManager.AddKeyPressCallback("p", PrintInfo);
keyboardManager.AddKeyPressCallback("q", Close);
PrintUsage();
// Set up PID controllers
jointPid = new Barrett.Control.PidVector(kpJoint, kiJoint, kdJoint, kDof, lowpassFilterFreq);
toolPid = new Barrett.Control.PidVector(kpTool, kiTool, kdTool, kNumDim, lowpassFilterFreq);
// Start the dtTimer
dtTimer.Reset();
dtTimer.Start();
// Loop: calculate forces/torques at every timestep based on current
// state feedback from the robot.
bool running = true;
intervalTimer.Reset();
while (running)
{
running = ReadKeyPress();
float dt = (float)dtTimer.ElapsedTicks / (float)Stopwatch.Frequency;
dtTimer.Restart();
if (jointHolding)
{
jointTorques = jointPid.Update(jointHoldPos, jointPos, dt);
toolForce.Clear();
}
else if (toolHolding)
{
toolForce = toolPid.Update(toolHoldPos, toolPos, dt);
jointTorques.Clear();
}
else
{
jointTorques.Clear();
toolForce.Clear();
}
robot.SendCartesianForcesAndJointTorques(toolForce.ToVector3(), jointTorques.ToVector3())
.Catch(e => Barrett.Logger.Debug(Barrett.Logger.CRITICAL, "Exception {0}", e))
.Done();
Thread.Sleep(Math.Max(0, controlLoopTime - (int)intervalTimer.ElapsedMilliseconds));
intervalTimer.Restart();
}
}
public RobotClientStarter()
{
this.Client = new RobotClient(true, 0);
}
public CustomJointTrajectory()
{
// Initialize vectors
jointPos = Vector <float> .Build.Dense(kDof);
jointTorque = 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);
// Set up communication with the robot and initialize force/torque to zero
robot = new RobotClient();
robot.SubscribeToServerUpdate(OnReceiveServerUpdate);
robot.SubscribeToRobotStatus(OnReceiveRobotStatus);
robot.SendCartesianForces(Vector3.zero);
// Set up keyboard callbacks
keyboardManager = new Barrett.KeyboardManager();
keyboardManager.SetDebug(true); // print key pressed
keyboardManager.AddKeyPressCallback("h", OnHome);
keyboardManager.AddKeyPressCallback("e", OnEnable);
keyboardManager.AddKeyPressCallback("d", OnDisable);
keyboardManager.AddKeyPressCallback("s", StartStop);
keyboardManager.AddKeyPressCallback("q", Close);
PrintUsage();
// Add some blank lines for readability, then get the cursor position.
Console.WriteLine();
Console.WriteLine();
int top = Console.CursorTop;
// Print labels, which do not change. Add an offset on the left side for readability
// and specify the length of the strings to clear any text that already exists in
// that space. Make sure that the length is longer than your strings to leave enough
// space.
int line = top;
int left = 10;
int length = 35;
PrintAtPosition(left, line++, "Commanded joint positions (rad):", length);
PrintAtPosition(left, line++, "Actual joint positions (rad):", length);
PrintAtPosition(left, line++, "Joint control torques (N-m):", length);
// Move the cursor to a nice place to display other text.
line += 2;
Console.SetCursorPosition(0, line);
// Set the start position on each line for the data.
left += length;
length = 30;
// Set up PID controller
jointPid = new Barrett.Control.PidVector(kpJoint, kiJoint, kdJoint, kDof, lowpassFilterFreq);
// Set up trajectory generator
startTraj = new Barrett.Control.LinearTrajectoryVector(kDof);
// Start the dtTimer
dtTimer.Reset();
dtTimer.Start();
// Loop: calculate forces/torques at every timestep based on current
// state feedback from the robot.
bool running = true;
intervalTimer.Reset();
displayTimer.Reset();
displayTimer.Start();
while (running)
{
running = ReadKeyPress();
float dt = (float)dtTimer.ElapsedTicks / (float)Stopwatch.Frequency;
dtTimer.Restart();
if (motionActive)
{
if (!startTraj.DoneMoving)
{
// Follow the linear trajectory to the start position until it is done.
startTraj.Update();
startTraj.Position.CopyTo(jointCommand);
}
else
{
// Start the timer for the circle, if it's not already started.
if (!circleTimer.IsRunning)
{
line += 1;
ClearLine(line, (uint)(Console.WindowHeight - line));
Console.SetCursorPosition(0, line);
Console.WriteLine("Executing circular movement.");
circleTimer.Start();
}
// Calculate the new joint position command. Constant in joint 0 and
// cyclic movement in joints 1 and 2.
float time = (float)(circleTimer.ElapsedMilliseconds) / 1000f;
jointCommand [0] = startPos [0];
jointCommand [1] = radius * (Mathf.Cos(2f * Mathf.PI * frequency * time) - 1.0f) + startPos [1];
jointCommand [2] = radius * Mathf.Sin(2f * Mathf.PI * frequency * time) + startPos [2];
}
jointTorque = jointPid.Update(jointCommand, jointPos, dt);
}
else
{
jointTorque.Clear();
}
robot.SendCartesianForcesAndJointTorques(Vector3.zero, jointTorque.ToVector3())
.Catch(e => Barrett.Logger.Debug(Barrett.Logger.CRITICAL, "Exception {0}", e))
.Done();
// Calculate how long to wait until next control cycle
Thread.Sleep(Math.Max(0, controlLoopTime - (int)intervalTimer.ElapsedMilliseconds));
intervalTimer.Restart();
// Update the display, if applicable.
if ((int)displayTimer.ElapsedMilliseconds >= displayUpdateTime)
{
line = top;
PrintAtPosition(left, line++, jointCommand.ToVector3().ToString("F4"), length);
PrintAtPosition(left, line++, jointPos.ToVector3().ToString("F4"), length);
PrintAtPosition(left, line++, jointTorque.ToVector3().ToString("F4"), length);
// Move the cursor to a nice place to display user input.
line += 2;
Console.SetCursorPosition(0, line);
displayTimer.Restart();
}
}
Console.Write("Quitting.");
Environment.Exit(0);
}
