Skip to content

Commit

Permalink
Merge pull request #66 from huskyroboticsteam/ConsoleViewFinalChanges
Browse files Browse the repository at this point in the history 
ConsoleView and file path manipulation improvements
  • Loading branch information
@Obbay2
Obbay2 authored Jul 21, 2018
2 parents 8626eca + 8fd6fdd commit 156fd6f
Show file tree
Hide file tree
Showing 7 changed files with 163 additions and 192 deletions.
202 changes: 90 additions & 112 deletions BaseStation/BaseServer/BaseServer.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,106 +50,93 @@ private static void ClientConnected(object sender, EventArgs e)
/// <param name="driveController"></param>
public static void Update(Controller driveController, Controller armController)
{
if (driveController.IsConnected
&& armController.IsConnected
&& (TimeNanoseconds() - lastControlSend) > CONTROL_SEND_INTERVAL_NANOSECONDS)
{
Packet SteerPack;
Packet SpeedPack;
Packet WristPack;
Packet ElbowPack;
Packet ShoulderPack;
Packet BasePack;

State driveState = driveController.GetState();
State armState = armController.GetState();
byte rightTrigger = driveState.Gamepad.RightTrigger;
byte leftTrigger = driveState.Gamepad.LeftTrigger;
short leftThumbX = PreventOverflow(driveState.Gamepad.LeftThumbX);

if (rightTrigger < TriggerThreshold) { rightTrigger = 0; }
if (leftTrigger < TriggerThreshold) { leftTrigger = 0; }
if (Math.Abs(leftThumbX) < LeftThumbDeadzone) { leftThumbX = 0; }

float speed = (float)UtilMain.LinearMap(rightTrigger - leftTrigger, -255, 255, -1, 1);
float steerPos = (float)UtilMain.LinearMap(leftThumbX, -32768, 32767, -1, 1);

//Console.WriteLine("Speed: " + speed);
//Console.WriteLine("Steer Pos: " + steerPos);

bool aPressedDrive = (driveState.Gamepad.Buttons & GamepadButtonFlags.A) != 0;
bool bPressedDrive = (driveState.Gamepad.Buttons & GamepadButtonFlags.B) != 0;

bool aPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.A) != 0;
bool bPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.B) != 0;
bool xPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.X) != 0;
bool yPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.Y) != 0;

bool upPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.DPadUp) != 0;
bool downPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.DPadDown) != 0;
bool leftPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.DPadLeft) != 0;
bool rightPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.DPadRight) != 0;

float steerSpeed = 0.0f;
if (aPressedDrive)
steerSpeed = 0.3f;
if (bPressedDrive)
steerSpeed = -0.3f;

float wristArmSpeed = 0.0f;
if (xPressedArm)
wristArmSpeed = 0.5f;
if (yPressedArm)
wristArmSpeed = -0.5f;

float elbowArmSpeed = 0.0f;
if (aPressedArm)
elbowArmSpeed = 0.5f;
if (bPressedArm)
elbowArmSpeed = -0.5f;

float shoulderArmSpeed = 0.0f;
if (upPressedArm)
shoulderArmSpeed = 1.0f;
if (downPressedArm)
shoulderArmSpeed = -1.0f;

float baseArmSpeed = 0.0f;
if (leftPressedArm)
baseArmSpeed = 0.5f;
if (rightPressedArm)
baseArmSpeed = -0.5f;

SteerPack = new Packet(0x8F, true, "MainRover");
SteerPack.AppendData(UtilData.ToBytes(steerSpeed));
Scarlet.Communications.Server.Send(SteerPack);

SpeedPack = new Packet(0x95, true, "MainRover");
SpeedPack.AppendData(UtilData.ToBytes(speed));
Scarlet.Communications.Server.Send(SpeedPack);

WristPack = new Packet(0x9D, true, "ArmMaster");
WristPack.AppendData(UtilData.ToBytes(wristArmSpeed));
Scarlet.Communications.Server.Send(WristPack);

ElbowPack = new Packet(0x9C, true, "ArmMaster");
ElbowPack.AppendData(UtilData.ToBytes(elbowArmSpeed));
Scarlet.Communications.Server.Send(ElbowPack);

ShoulderPack = new Packet(0x9B, true, "ArmMaster");
ShoulderPack.AppendData(UtilData.ToBytes(shoulderArmSpeed));
Scarlet.Communications.Server.Send(ShoulderPack);

BasePack = new Packet(0x9A, true, "ArmMaster");
BasePack.AppendData(UtilData.ToBytes(baseArmSpeed));
Scarlet.Communications.Server.Send(BasePack);

lastControlSend = TimeNanoseconds(); //time in nanoseconds
}
else if ((TimeNanoseconds() - lastControlSend) > CONTROL_SEND_INTERVAL_NANOSECONDS)
{
Console.WriteLine("Gamepad not connected");
HaltRoverMotion();
if(SendIntervalElapsed()) {
if(driveController.IsConnected && armController.IsConnected) {
State driveState = driveController.GetState();
State armState = armController.GetState();
byte rightTrigger = driveState.Gamepad.RightTrigger;
byte leftTrigger = driveState.Gamepad.LeftTrigger;
short leftThumbX = PreventOverflow(driveState.Gamepad.LeftThumbX);

if (rightTrigger < TriggerThreshold) { rightTrigger = 0; }
if (leftTrigger < TriggerThreshold) { leftTrigger = 0; }
if (Math.Abs(leftThumbX) < LeftThumbDeadzone) { leftThumbX = 0; }

float speed = (float)UtilMain.LinearMap(rightTrigger - leftTrigger, -255, 255, -1, 1);
float steerPos = (float)UtilMain.LinearMap(leftThumbX, -32768, 32767, -1, 1);

bool aPressedDrive = (driveState.Gamepad.Buttons & GamepadButtonFlags.A) != 0;
bool bPressedDrive = (driveState.Gamepad.Buttons & GamepadButtonFlags.B) != 0;

bool aPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.A) != 0;
bool bPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.B) != 0;
bool xPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.X) != 0;
bool yPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.Y) != 0;

bool upPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.DPadUp) != 0;
bool downPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.DPadDown) != 0;
bool leftPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.DPadLeft) != 0;
bool rightPressedArm = (armState.Gamepad.Buttons & GamepadButtonFlags.DPadRight) != 0;

float steerSpeed = 0.0f;
if (bPressedDrive)
steerSpeed = -0.3f;
else if (aPressedDrive)
steerSpeed = 0.3f;

float wristArmSpeed = 0.0f;
if (yPressedArm)
wristArmSpeed = -0.5f;
else if (xPressedArm)
wristArmSpeed = 0.5f;

float elbowArmSpeed = 0.0f;
if (bPressedArm)
elbowArmSpeed = -0.5f;
else if (aPressedArm)
elbowArmSpeed = 0.5f;

float shoulderArmSpeed = 0.0f;
if (downPressedArm)
shoulderArmSpeed = -1.0f;
else if (upPressedArm)
shoulderArmSpeed = 1.0f;

float baseArmSpeed = 0.0f;
if (rightPressedArm)
baseArmSpeed = -0.5f;
else if (leftPressedArm)
baseArmSpeed = 0.5f;

Packet SteerPack = new Packet(0x8F, true, "MainRover");
SteerPack.AppendData(UtilData.ToBytes(steerSpeed));
Scarlet.Communications.Server.Send(SteerPack);

Packet SpeedPack = new Packet(0x95, true, "MainRover");
SpeedPack.AppendData(UtilData.ToBytes(speed));
Scarlet.Communications.Server.Send(SpeedPack);

Packet WristPack = new Packet(0x9D, true, "ArmMaster");
WristPack.AppendData(UtilData.ToBytes(wristArmSpeed));
Scarlet.Communications.Server.Send(WristPack);

Packet ElbowPack = new Packet(0x9C, true, "ArmMaster");
ElbowPack.AppendData(UtilData.ToBytes(elbowArmSpeed));
Scarlet.Communications.Server.Send(ElbowPack);

Packet ShoulderPack = new Packet(0x9B, true, "ArmMaster");
ShoulderPack.AppendData(UtilData.ToBytes(shoulderArmSpeed));
Scarlet.Communications.Server.Send(ShoulderPack);

Packet BasePack = new Packet(0x9A, true, "ArmMaster");
BasePack.AppendData(UtilData.ToBytes(baseArmSpeed));
Scarlet.Communications.Server.Send(BasePack);
} else {
Console.WriteLine("Gamepad not connected!");
HaltRoverMotion();
}

lastControlSend = TimeNanoseconds();
}
}

Expand All @@ -167,19 +154,10 @@ private static void HaltRoverMotion()
Scarlet.Communications.Server.Send(ArmEmergencyStop);
}

private static short PreventOverflow(short shortVal)
{
if (shortVal == -32768)
{
shortVal++;
}
return shortVal;
}
private static short PreventOverflow(short shortVal) => shortVal == -32768 ? (short)(shortVal + 1) : shortVal;

private static long TimeNanoseconds()
{
return DateTime.UtcNow.Ticks * 100;
}
private static bool SendIntervalElapsed() => (TimeNanoseconds() - lastControlSend) > CONTROL_SEND_INTERVAL_NANOSECONDS;
private static long TimeNanoseconds() => DateTime.UtcNow.Ticks * 100;

private static List<float> ConvertToFloatArray(Packet data)
{
Expand Down
2 changes: 1 addition & 1 deletion BaseStation/BaseStation/BaseStation.csproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
<ProjectGuid>{717939F0-9BDC-4D66-99B5-AE34D586488E}</ProjectGuid>
<OutputType>WinExe</OutputType>
<OutputType>Exe</OutputType>
<RootNamespace>HuskyRobotics.BaseStation</RootNamespace>
<AssemblyName>BaseStation</AssemblyName>
<TargetFrameworkVersion>v4.7</TargetFrameworkVersion>
Expand Down
2 changes: 1 addition & 1 deletion BaseStation/BaseStation/StartBaseStation.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ private static void Update()
{
BaseServer.Update(DriveController, ArmController);
CameraControl.Update(DriveController);
Thread.Sleep(150);
Thread.Sleep(100);
}

private static volatile bool exit;
Expand Down
87 changes: 42 additions & 45 deletions BaseStation/MainWindow/ConsoleView.xaml.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,84 +2,82 @@
using System.IO;
using System.Text;
using System.Windows.Controls;
using System.Windows.Media;

namespace HuskyRobotics.UI
{
/// <summary>
/// Interaction logic for ConsoleView.xaml
/// </summary>
public partial class ConsoleView : ScrollViewer
{
public ConsoleView()
{
TextBlock child = new TextBlock();
AddChild(child);
child.FontFamily = new System.Windows.Media.FontFamily("consolas");
child.FontSize = 16;
Console.SetOut(new ConsoleWriter(child, this, Console.Out));
public partial class ConsoleView : ScrollViewer {
private readonly ConsoleWriter writer;

public TextWriter Writer => writer;

public ConsoleView() {
TextBox box = new TextBox {
FontFamily = new FontFamily("consolas"),
FontSize = 16,
IsReadOnly = true,
IsReadOnlyCaretVisible = false,
BorderThickness = new System.Windows.Thickness(0),
TextWrapping = System.Windows.TextWrapping.Wrap
};

AddChild(box);
writer = new ConsoleWriter(box, this);
InitializeComponent();
}

private class ConsoleWriter : TextWriter
{
private readonly TextBlock view;
private readonly ScrollViewer scroll;
private readonly TextWriter oldOut;
private class ConsoleWriter : TextWriter {
private readonly TextBox view;
private readonly ScrollViewer scroll;

public ConsoleWriter(TextBlock view, ScrollViewer scroll, TextWriter oldOut)
{
this.view = view;
this.scroll = scroll;
this.oldOut = oldOut;
view.TextWrapping = System.Windows.TextWrapping.Wrap;
}
public ConsoleWriter(TextBox view, ScrollViewer scroll) {
this.view = view;
this.scroll = scroll;
}

public override Encoding Encoding => Encoding.UTF8;

public override void Write(char value)
{
oldOut.Write(value);
view.Dispatcher.BeginInvoke(new Action(() =>
{
view.Text += value;
view.Dispatcher.InvokeAsync(() => {
view.AppendText(value.ToString());
UpdateView();
}));
});
}

public override void Write(char[] buffer, int index, int count)
{
oldOut.Write(buffer, index, count);
view.Dispatcher.BeginInvoke(new Action(() =>
{
view.Text += new string(buffer, index, count);
view.Dispatcher.InvokeAsync(() => {
view.AppendText(new string(buffer, index, count));
UpdateView();
}));
});
}

//removes extra text (prevent memory leak)
//and scrolls view to bottom (like a console)
private void UpdateView()
{
int len = view.Text.Length;
private void UpdateView() {
int len = view.Text.Length;
double scrollHeight = scroll.ViewportHeight;
double viewHeight = view.ActualHeight;
int totalLines = CountLines(view.Text);
int lineHeight = (int)viewHeight / totalLines;
int possibleVisibleLines = (int)(scrollHeight / lineHeight);
if (viewHeight > scrollHeight * 2) //if stored text is partially offscreen, the 2 is there to buffer the text so there isn't so much garbage produced
{
if (viewHeight > scrollHeight) {
view.Text = GetFinalLines(view.Text, totalLines, Math.Min(totalLines, possibleVisibleLines));
}
scroll.ScrollToEnd();
}
scroll.ScrollToEnd();
}

private static string GetFinalLines(string text, int totalLines, int lineCount)
{
int ind = GetIndexAfter(text, totalLines - lineCount);
int ind = GetIndexAfterNewline(text, totalLines - lineCount);
return text.Substring(ind);
}

private static int GetIndexAfter(string str, int count)
private static int GetIndexAfterNewline(string str, int count)
{
if (str == string.Empty) return -1;
int index = -1;
Expand All @@ -95,12 +93,11 @@ private static int CountLines(string str)
if (str == string.Empty) return 0;
int index = -1;
int count = 0;
while (-1 != (index = str.IndexOf(Environment.NewLine, index + Environment.NewLine.Length)))
{
count++;
}
while (-1 != (index = str.IndexOf(Environment.NewLine, index + Environment.NewLine.Length))) {
count++;
}
return count + 1;
}
}
}
}
}
2 changes: 1 addition & 1 deletion BaseStation/MainWindow/MainWindow.xaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@
<ui:SettingsPanel x:Name="SettingPanel"/>
</TabItem>
<TabItem Header="Console">
<ui:ConsoleView Grid.Column="3"></ui:ConsoleView>
<ui:ConsoleView x:Name="console" Grid.Column="3"/>
</TabItem>
</TabControl>
</DockPanel>
Expand Down
Loading

0 comments on commit 156fd6f

Please sign in to comment.