/**
*
*/
private void DeviceList_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
try
{
//Stop the stream of the previously selected event
foreach (PiCarConnection oldPicar in e.RemovedItems)
{
oldPicar.StopStream();
clearStreamImage();
}
}
catch (Exception exception)
{
//TODO Remove vehicles that throw exceptions
LogField.AppendText(DateTime.Now + ":\tException found when removing an old streams!\n" + e + "\n");
//TODO remove previous car
Console.WriteLine(exception);
}
var picar = SelectedPiCar();
if (picar is null)
{
return;
}
StreamToggle.IsEnabled = true;
StreamToggle.IsChecked = false;
//Update ipBox and deviceStatus with it's info
IpBox.Text = picar.ipAddress.ToString();
DeviceStatus.Text = picar.Mode.ToString();
}
/**
* Method that handles when the GUI buttons are held down (Vehicle is moving a single direction)
*/
private void ButtonPress_Event(object sender, RoutedEventArgs e)
{
//TODO add button up event for stop command
//var picar = (PiCarConnection)DeviceListMn.SelectedItem;
//if (picar == null || picar.Mode != ModeRequest.Types.Mode.Lead) return;
var button = (RepeatButton)sender;
switch (button.Name)
{
case "Forward":
LogField.AppendText(DateTime.Now + ":\tMoving forward\n");
MoveVehicle(1.0, 0.0);
break;
case "Backwards":
LogField.AppendText(DateTime.Now + ":\tMoving backwards\n");
MoveVehicle(-1.0, 0.0);
break;
case "Left":
LogField.AppendText(DateTime.Now + ":\tMoving left\n");
MoveVehicle(0.0, -1.0);
break;
case "Right":
LogField.AppendText(DateTime.Now + ":\tMoving right\n");
MoveVehicle(0.0, 1.0);
break;
default:
Console.WriteLine("Mistakes were made");
break;
}
LogField.ScrollToEnd();
}
/**
* Method that handles when one or more key is released (Vehicle is stopping in one or more directions)
*/
private void Key_up(object sender, KeyEventArgs e)
{
//var picar = (PiCarConnection)DeviceListMn.SelectedItem;
//if (picar == null || picar.Mode != ModeRequest.Types.Mode.Lead) return;
var directionMotor = 0.0;
var throttleMotor = 0.0;
if (Keyboard.IsKeyUp(Key.W) && Keyboard.IsKeyUp(Key.Up))
{
throttleMotor--;
}
if (Keyboard.IsKeyUp(Key.S) && Keyboard.IsKeyUp(Key.Down))
{
throttleMotor++;
}
if (Keyboard.IsKeyUp(Key.A) && Keyboard.IsKeyUp(Key.Left))
{
directionMotor++;
}
if (Keyboard.IsKeyUp(Key.D) && Keyboard.IsKeyUp(Key.Right))
{
directionMotor--;
}
string output = Direction.EncodeDirection(DateTime.Now, throttleMotor, directionMotor);
LogField.AppendText(output);
MoveVehicle(throttleMotor, directionMotor);
LogField.ScrollToEnd();
}
/**
* Method that handles when the GUI button is released (Vehicle is stopped)
*/
private void ButtonPress_Released(object sender, RoutedEventArgs e)
{
//var picar = (PiCarConnection)DeviceListMn.SelectedItem;
//if (picar == null || picar.Mode != ModeRequest.Types.Mode.Lead) return;
LogField.AppendText(DateTime.Now + ":\tNow In Neutral\n");
MoveVehicle(0.0, 0.0);
LogField.ScrollToEnd();
}
//stops the stream from being saved
private void StopSaving_Click(object sender, RoutedEventArgs e)
{
StreamSavingHeader.IsEnabled = true;
StopStreamSavingHeader.IsEnabled = false;
if (_saveStreamEnabled == true)
{
LogField.AppendText(DateTime.Now + ":\tStream will no longer be saved to a file\n");
LogField.ScrollToEnd();
}
_saveStreamEnabled = false;
}
/**
* Clear the image that was displayed by the stream
*/
public void clearStreamImage()
{
try
{
synchronizationContext.Post(o => StreamImage.Source = (ImageSource)o, null);
}
catch (Exception e)
{
LogField.AppendText($"{DateTime.Now}:\tError clearing stream image: {e}\n");
}
}
private void SetFollowerModelPID_Click(object sender, RoutedEventArgs e)
{
var picar = SelectedPiCar();
if (picar is null)
{
return;
}
LogField.AppendText(DateTime.Now + ":\tSetting " + picar + "to legacy PID follower model\n");
LogField.ScrollToEnd();
picar.SetFollowerModel(1);
}
/**
* Method that runs when the main window launches
*/
public MainWindow()
{
InitializeComponent();
//Setup sync context
synchronizationContext = SynchronizationContext.Current;
Title = "Welcome " + Environment.UserName;
//Adds shortcut to open the registration window with Ctrl + R
var newKeybind = new RoutedCommand();
newKeybind.InputGestures.Add(new KeyGesture(Key.R, ModifierKeys.Control));
CommandBindings.Add(new CommandBinding(newKeybind, Register_Click));
_controlMode = true;
_saveStreamEnabled = false;
//Adds shortut Ctrl + S for stream saving and Ctrl + D for disabling stream saving
var streamKeybind = new RoutedCommand();
streamKeybind.InputGestures.Add(new KeyGesture(Key.S, ModifierKeys.Control));
CommandBindings.Add(new CommandBinding(streamKeybind, ImageSaving_Click));
var dStreamKeybind = new RoutedCommand();
dStreamKeybind.InputGestures.Add(new KeyGesture(Key.D, ModifierKeys.Control));
CommandBindings.Add(new CommandBinding(dStreamKeybind, StopSaving_Click));
//Checks if a controller is plugged into the current OS
_controller = new Controller(UserIndex.One);
if (!_controller.IsConnected)
{
LogField.AppendText(DateTime.Now + ":\tNo controller found!\n");
}
else
{
//Uses a timer to loop a method that checks the status of the controller
LogField.AppendText(DateTime.Now + ":\tController detected!\n");
_timer = new DispatcherTimer {
Interval = TimeSpan.FromSeconds(1 / 30)
};
_timer.Tick += _timer_Tick;
_timer.Start();
_directionController = 0.0;
_throttleController = 0.0;
}
//sets initial connection configuration specified by .ini file
initializeUI();
}
//tries to connect to cars specified in .ini file
private async Task IPConnect(string selectedIP, string selectedName)
{
//Handle the dummy connection
if (selectedIP == "DummyIP")
{
var dummyConnection = new DummyConnection(selectedName, selectedIP);
deviceListMain.Add(dummyConnection);
LogField.AppendText(DateTime.Now + ":\t" + "Added " + selectedName + " for testing\n");
//LogFieldReg.AppendText("Added " + selectedName + " for testing\n");
}
else if (!CheckIfValidIP(selectedIP))
{
//LogFieldReg.AppendText("Invalid IP used, try again!\n");
LogField.AppendText(DateTime.Now + ":\tInvalid IP used, try again!\n");
}
else
{
PiCarConnection newConnection = null;
var canConnect = false;
try
{
newConnection = new PiCarConnection(selectedName, selectedIP);
var connectResponse = newConnection.RequestConnect();
Console.Write(connectResponse.Item2);
//LogFieldReg.AppendText(connectResponse.Item2);
LogField.AppendText(DateTime.Now + ":\t" + connectResponse.Item2);
canConnect = connectResponse.Item1;
}
catch (RpcException rpcE)
{
LogField.AppendText(DateTime.Now + ":\tRPC error: " + rpcE.Message + "\n");
}
catch (Exception exception)
{
LogField.AppendText(DateTime.Now + ":\tError! " + exception + "\n");
}
if (canConnect)
{
LogField.AppendText(DateTime.Now + ":\t" + "Connected to " + selectedName + " with IP: " + selectedIP + "\n");
//LogFieldReg.AppendText("Connected to " + selectedName + " with IP: " + selectedIP + "\n");
deviceListMain.Add(newConnection);
}
else
{
LogField.AppendText(DateTime.Now + ":\t" + "Failed to connect to " + selectedName + " with IP: " + selectedIP + "\n");
//LogFieldReg.AppendText("Failed to connect to " + selectedName + " with IP: " + selectedIP + "\n");
}
}
}
/**
* Timer method that calls the method that checks the controller status
*/
private void _timer_Tick(object sender, EventArgs e)
{
try
{
ControllerMovement();
}
catch (Exception exception)
{
Console.WriteLine(exception);
LogField.AppendText(DateTime.Now + ":\tController disconnected\n");
_timer.Stop();
}
}
private void DisconnectCar(PiCarConnection picar)
{
if (picar.GetType() == typeof(DummyConnection))
{
return;
}
LogField.AppendText(DateTime.Now + ":\t" + picar + " stopped responding, disconnecting.\n");
LogField.ScrollToEnd();
deviceListMain.Remove(picar);
DeviceListMn.ItemsSource = null;
DeviceListMn.ItemsSource = deviceListMain;
}
/**
* Update method which gets called by PiCarConnection when sending image frames and car actions.
*/
public void HandleStream(byte[] imageBytes, SetMotion action)
{
if (imageBytes == null)
{
return;
}
var save_dir_path = getPathName();
var session_prefix = getSessionName();
bool save_to_disk = getSaveEnabled();
//Convert bytes to ImageSource type for GUI
var imgSource = (ImageSource) new ImageSourceConverter().ConvertFrom(imageBytes);
// Update the GUI
try
{
synchronizationContext.Post(o => StreamImage.Source = (ImageSource)o, imgSource);
}
catch (Exception e)
{
LogField.AppendText($"{DateTime.Now}: Error updating the GUI with image received from car: {e}\n");
}
if (save_to_disk)
{
try
{
var csv_path = $"{save_dir_path}\\{session_prefix}.csv";
var image_file_name = $"{session_prefix}_{saved_frame_count.ToString("D5")}.jpg";
saved_frame_count += 1;
using (var fileStream = new FileStream($"{save_dir_path}\\train\\{image_file_name}", FileMode.Create))
{
//Console.WriteLine($"Writing image of length {imageBytes.Length}");
fileStream.Write(imageBytes, 0, imageBytes.Length);
fileStream.Flush();
}
using (var streamWriter = new StreamWriter(csv_path, true))
{
streamWriter.WriteLineAsync($"train/{image_file_name},{action.Throttle},{action.Direction}");
}
}
catch (IOException e)
{
LogField.AppendText($"{DateTime.Now}:\tError writing stream data to disk: {e.Message}\n");
}
}
}
private void SetVehicleMode(PiCarConnection picar, ModeRequest.Types.Mode mode)
{
try
{
picar.SetMode(mode);
DeviceStatus.Text = picar.Mode.ToString();
LogField.AppendText(DateTime.Now + ":\tSetting " + picar + "to " + picar.Mode.ToString() + "\n");
LogField.ScrollToEnd();
}
catch (Exception e)
{
DisconnectCar(picar);
Console.WriteLine(e);
}
}
public void writeStreamCsvHeader()
{
var save_dir_path = getPathName();
var session_prefix = getSessionName();
var csv_path = $"{save_dir_path}\\{session_prefix}.csv";
if (!File.Exists(csv_path))
{
try {
using (var streamWriter = new StreamWriter(csv_path, true)) {
streamWriter.WriteLineAsync($"image_file,throttle,direction");
}
}
catch (IOException e) {
LogField.AppendText($"{DateTime.Now}:\tError opening csv: {e.Message}\n");
}
}
}
private void ModeChanger_Click(object sender, RoutedEventArgs e)
{
if (_controlMode)
{
_controlMode = false;
DefaultHeader.IsEnabled = true;
AlternativeHeader.IsEnabled = false;
LogField.AppendText(DateTime.Now + ":\tUsing RC control mode\n");
}
else
{
_controlMode = true;
DefaultHeader.IsEnabled = false;
AlternativeHeader.IsEnabled = true;
LogField.AppendText(DateTime.Now + ":\tUsing simulator control mode\n");
}
LogField.ScrollToEnd();
}
/**
* Method that handles when one or more key is pressed down (Vehicle is moving in one or more directions)
*/
private void Key_down(object sender, KeyEventArgs e)
{
//var picar = (PiCarConnection)DeviceListMn.SelectedItem;
//if (picar == null || picar.Mode != ModeRequest.Types.Mode.Lead) return;
if (e.IsRepeat)
{
return;
}
var directionMotor = 0.0;
var throttleMotor = 0.0;
string[] throttleStrings = { "Moving backwards", "In Neutral", "Moving forwards" };
string[] directionStrings = { "and left", "", "and right" };
if (Keyboard.IsKeyDown(Key.W) || Keyboard.IsKeyDown(Key.Up))
{
throttleMotor++;
}
if (Keyboard.IsKeyDown(Key.S) || Keyboard.IsKeyDown(Key.Down))
{
throttleMotor--;
}
if (Keyboard.IsKeyDown(Key.A) || Keyboard.IsKeyDown(Key.Left))
{
directionMotor--;
}
if (Keyboard.IsKeyDown(Key.D) || Keyboard.IsKeyDown(Key.Right))
{
directionMotor++;
}
string output = Direction.EncodeDirection(DateTime.Now, throttleMotor, directionMotor);
LogField.AppendText(output);
MoveVehicle(throttleMotor, directionMotor);
LogField.ScrollToEnd();
}
/**
* Method that handles shutdown confirmation
*/
private void Window_Closing(object sender, CancelEventArgs e)
{
foreach (var t in DeviceListMn.Items)
{
try
{
if (!(t is PiCarConnection temp) || temp.Mode != ModeRequest.Types.Mode.Lead)
{
continue;
}
LogField.AppendText(DateTime.Now + ":\t" + temp.Name + " is stopping");
clearStreamImage();
temp.StopStream();
MoveVehicle(0.0, 0.0);
SetVehicleMode(ModeRequest.Types.Mode.Idle);
}
catch (Exception exception)
{
LogField.AppendText(DateTime.Now + ":\tSomething went wrong: " + exception.ToString());
}
}
Application.Current.Shutdown();
}
//sets up initia configuration for connection and log using specified .ini file
private async void initializeUI()
{
ArrayList carInfoArray = new ArrayList();
var file_path = $"{Environment.GetFolderPath(Environment.SpecialFolder.MyDocuments)}\\picar\\gui_config.ini";
if (!File.Exists(file_path))
{
return;
}
//gets text from specified .ini file
try {
string[] lines = File.ReadAllLines(file_path);
string selectedIP;
string selectedName;
string[] ipNameAndMode;
string mode;
string path;
string session;
string[] path_and_session;
for (int i = 0; i < lines.Length; i++)
{
if (lines[i].StartsWith("#"))
{
continue;
}
;
if (lines[i] == "(*connect)")
{
while (lines[i + 1] != "(connect*)")
{
ipNameAndMode = lines[i + 1].Split(',');
selectedIP = ipNameAndMode[0];
selectedName = ipNameAndMode[1];
mode = ipNameAndMode[2];
carInfoArray.Add(ipNameAndMode);
await IPConnect(selectedIP, selectedName);
i = i + 1;
}
}
if (lines[i] == "(*stream)")
{
while (lines[i + 1] != "(stream*)")
{
path_and_session = lines[i + 1].Split(',');
path = path_and_session[0];
session = path_and_session[1];
setPathName(path);
setSessionName(session);
i = i + 1;
}
}
if (lines[i] == "(*log)")
{
while (lines[i + 1] != "(log*)")
{
LogField.AppendText(lines[i + 1] + "\n");
i = i + 1;
}
}
}
}
catch (Exception e)
{
LogField.AppendText($"{DateTime.Now}:\tError when initializing configuration: {e.Message}\n");
}
DeviceListMn.ItemsSource = null;
DeviceListMn.ItemsSource = deviceListMain;
//initializes mode for each connection in .ini
foreach (string[] m in carInfoArray)
{
initializeMode(m[1], m[2]);
}
}
/**
* Method that handles the simulator style input for variable speed and direction
*/
private void ControllerMovement()
{
//var picar = (PiCarConnection)DeviceListMn.SelectedItem;
//if (picar == null || picar.Mode != ModeRequest.Types.Mode.Lead) return;
var state = _controller.GetState().Gamepad;
//Default control settings (Simulator Mode)
if (_controlMode)
{
if (state.LeftThumbX.Equals(_previousState.LeftThumbX) &&
state.LeftTrigger.Equals(_previousState.LeftTrigger) &&
state.RightTrigger.Equals(_previousState.RightTrigger))
{
return;
}
//_Motor1 produces either -1.0 for left or 1.0 for right motion
_directionController = Math.Abs((double)state.LeftThumbX) < DeadzoneValue
? 0 :
(double)state.LeftThumbX / short.MinValue * -1;
_directionController = Math.Round(_directionController, 3);
/**
* These variables produce either 1.0 for forward motion, or -1 for backwards.
* If the values are both non-zero, then there will be no motion in either direction
*/
var forwardSpeed = Math.Round(state.RightTrigger / 255.0, 3);
var backwardSpeed = Math.Round(state.LeftTrigger / 255.0 * -1.0, 3);
if (forwardSpeed > 0 && backwardSpeed == 0)
{
_throttleController = forwardSpeed;
}
else if (backwardSpeed < 0 && forwardSpeed == 0)
{
_throttleController = backwardSpeed;
}
else
{
_throttleController = 0.0;
}
}
//Alternative control settings (RC Mode)
else
{
if (state.LeftThumbY.Equals(_previousState.LeftThumbY) &&
state.RightThumbX.Equals(_previousState.RightThumbX))
{
return;
}
_directionController = Math.Abs((double)state.RightThumbX) < DeadzoneValue
? 0
: (double)state.RightThumbX / short.MinValue * -1;
_directionController = Math.Round(_directionController, 3);
_throttleController = Math.Abs((double)state.LeftThumbY) < DeadzoneValue
? 0
: (double)state.LeftThumbY / short.MinValue * -1;
}
string output = Direction.EncodeDirection(DateTime.Now, _throttleController, _directionController);
LogField.AppendText(output);
LogField.ScrollToEnd();
MoveVehicle(_throttleController, _directionController);
_previousState = state;
}
