diff --git a/ADIS_Csharp/ADIS_Csharp.sln b/ADIS_Csharp/ADIS_Csharp.sln index bd84a1d..2637d30 100644 --- a/ADIS_Csharp/ADIS_Csharp.sln +++ b/ADIS_Csharp/ADIS_Csharp.sln @@ -19,9 +19,11 @@ Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MqttTest", "MqttTest\MqttTe EndProject Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "RobotLib", "RobotLib\RobotLib.csproj", "{915E2889-F10D-4D02-8313-308F642EC64F}" EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotConsoleClient", "RobotConsoleClient\RobotConsoleClient.csproj", "{7FFC4B71-FFC2-45A1-941D-4B0A78C20B89}" +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "RobotConsoleClient", "RobotConsoleClient\RobotConsoleClient.csproj", "{7FFC4B71-FFC2-45A1-941D-4B0A78C20B89}" EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotClientWpf", "RobotClientWpf\RobotClientWpf.csproj", "{1D715C65-A8D8-45D3-AE63-D88C60337505}" +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "RobotClientWpf", "RobotClientWpf\RobotClientWpf.csproj", "{1D715C65-A8D8-45D3-AE63-D88C60337505}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RaspiControl", "RaspiControl\RaspiControl.csproj", "{CE8FA2F2-2DE4-40B6-A933-988E135C5039}" EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution @@ -69,6 +71,10 @@ Global {1D715C65-A8D8-45D3-AE63-D88C60337505}.Debug|Any CPU.Build.0 = Debug|Any CPU {1D715C65-A8D8-45D3-AE63-D88C60337505}.Release|Any CPU.ActiveCfg = Release|Any CPU {1D715C65-A8D8-45D3-AE63-D88C60337505}.Release|Any CPU.Build.0 = Release|Any CPU + {CE8FA2F2-2DE4-40B6-A933-988E135C5039}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {CE8FA2F2-2DE4-40B6-A933-988E135C5039}.Debug|Any CPU.Build.0 = Debug|Any CPU + {CE8FA2F2-2DE4-40B6-A933-988E135C5039}.Release|Any CPU.ActiveCfg = Release|Any CPU + {CE8FA2F2-2DE4-40B6-A933-988E135C5039}.Release|Any CPU.Build.0 = Release|Any CPU EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/ADIS_Csharp/RaspiControl/Joystick.cs b/ADIS_Csharp/RaspiControl/Joystick.cs new file mode 100644 index 0000000..6c38798 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/Joystick.cs @@ -0,0 +1,66 @@ +using System; +using System.Collections.Generic; +using System.Threading; +using Unosquare.RaspberryIO; +using Unosquare.RaspberryIO.Abstractions; +using Unosquare.WiringPi; + +namespace RaspiControl { + public class Joystick { + + private static readonly IGpioPin joystickUpPin = Pi.Gpio[BcmPin.Gpio19]; + private static readonly IGpioPin joysticDownPin = Pi.Gpio[BcmPin.Gpio13]; + private static readonly IGpioPin joystickLeftPin = Pi.Gpio[BcmPin.Gpio06]; + private static readonly IGpioPin joystickRightPin = Pi.Gpio[BcmPin.Gpio05]; + private static readonly IGpioPin joystickPushPin = Pi.Gpio[BcmPin.Gpio26]; + + private static readonly List inputPins = new List { joystickUpPin, joysticDownPin, + joystickLeftPin, joystickRightPin, joystickPushPin }; + + #region Properties + public JoystickButton State { + get { + JoystickButton state = JoystickButton.None; + if (!joystickLeftPin.Read()) state |= JoystickButton.Left; + if (!joystickRightPin.Read()) state |= JoystickButton.Right; + if (!joystickUpPin.Read()) state |= JoystickButton.Up; + if (!joysticDownPin.Read()) state |= JoystickButton.Down; + if (!joystickPushPin.Read()) state |= JoystickButton.Center; + return state; + } + } + #endregion + + #region Events + public event EventHandler? JoystickChanged; + + private void OnJoystickChanged(JoystickButton state) { + Console.WriteLine($"Joystick was pushed: {state}"); + this.JoystickChanged?.Invoke(this, new JoystickEventArgs(state)); + } + #endregion + + public Joystick() { + Pi.Init(); + foreach (var pin in inputPins) { + pin.PinMode = GpioPinDriveMode.Input; + } + + Thread thread = new Thread(Run); + thread.IsBackground = true; + thread.Start(); + } + + private void Run() { + JoystickButton oldState = State; + while (true) { + JoystickButton newState = State; + if (oldState != newState) { + oldState = newState; + OnJoystickChanged(newState); + } + Thread.Sleep(50); + } + } + } +} diff --git a/ADIS_Csharp/RaspiControl/JoystickButton.cs b/ADIS_Csharp/RaspiControl/JoystickButton.cs new file mode 100644 index 0000000..18e717e --- /dev/null +++ b/ADIS_Csharp/RaspiControl/JoystickButton.cs @@ -0,0 +1,18 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace RaspiControl { + + public enum JoystickButton { + None = 0, + Left = 1, + Right = 2, + Up = 4, + Down = 8, + Center = 16 + } + +} diff --git a/ADIS_Csharp/RaspiControl/JoystickEventArgs.cs b/ADIS_Csharp/RaspiControl/JoystickEventArgs.cs new file mode 100644 index 0000000..015f297 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/JoystickEventArgs.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace RaspiControl { + public class JoystickEventArgs : EventArgs { + + public JoystickButton Button { get; } + public JoystickEventArgs(JoystickButton button) { + Button = button; + } + } +} diff --git a/ADIS_Csharp/RaspiControl/Navigation.cs b/ADIS_Csharp/RaspiControl/Navigation.cs new file mode 100644 index 0000000..1510584 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/Navigation.cs @@ -0,0 +1,53 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace RaspiControl { + internal class Navigation { + private int angleRight; + private int angleLeft; + public Navigation() { + this.angleRight = 0; + this.angleLeft = 0; + } + + public void ResetAngle() { + this.angleRight = 0; + this.angleRight= 0; + } + + public int GetAngleLeftStep() { + this.angleLeft -= 10; + if (this.angleLeft < -180) { + this.angleLeft = -180; + } + return -10; + } + + public int GetAngleRightStep() { + this.angleRight += 10; + if (this.angleRight > 180) { + this.angleRight = 180; + } + return 10; + } + + public int IncreaseAngleLeft() { + this.angleLeft -= 10; + if(this.angleLeft < -180) { + this.angleLeft = -180; + } + return this.angleLeft; + } + + public int IncreaseAngleRight() { + this.angleRight += 10; + if (this.angleRight > 180) { + this.angleRight = 180; + } + return this.angleRight; + } + } +} diff --git a/ADIS_Csharp/RaspiControl/Program.cs b/ADIS_Csharp/RaspiControl/Program.cs new file mode 100644 index 0000000..262d227 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/Program.cs @@ -0,0 +1,54 @@ +using System; +using System.Text; +using M2Mqtt; +using M2Mqtt.Messages; +using Swan.Formatters; + +namespace RaspiControl { + class Programm { + private static MqttClient client; + private static Navigation navigation; + private static Joystick joystick; + static void Main(string[] args) { + try { + client = new MqttClient("localhost"); + client.MqttMsgPublishReceived += Client_MqttMsgPublishReceived; + string clientId = Guid.NewGuid().ToString(); + client.Connect(clientId); + client.Subscribe(new string[] { "APROG/REQUEST" }, new byte[] { MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE }); + navigation= new Navigation(); + joystick= new Joystick(); + joystick.JoystickChanged += Joystick_JoystickChanged; + + } catch (Exception ex) { + Console.WriteLine(ex.Message); + } + } + + private static void Joystick_JoystickChanged(object sender, JoystickEventArgs e) { + switch (e.Button) { + case JoystickButton.None: + break; + case JoystickButton.Left: + client.Publish("mobile/cmd/nav/left", Encoding.UTF8.GetBytes($"{navigation.GetAngleLeftStep()}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); + break; + case JoystickButton.Right: + client.Publish("mobile/cmd/nav/right", Encoding.UTF8.GetBytes($"{navigation.GetAngleRightStep()}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); + break; + case JoystickButton.Up: + break; + case JoystickButton.Down: + break; + case JoystickButton.Center: + navigation.ResetAngle(); + client.Publish("mobile/cmd/nav/drive", Encoding.UTF8.GetBytes("true"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); + break; + } + } + + private static void Client_MqttMsgPublishReceived(object sender, MqttMsgPublishEventArgs e) { + Console.Write(e.Topic + "\n"); + Console.Write(Encoding.UTF8.GetString(e.Message) + "\n"); + } + } +} \ No newline at end of file diff --git a/ADIS_Csharp/RaspiControl/RaspiControl.csproj b/ADIS_Csharp/RaspiControl/RaspiControl.csproj new file mode 100644 index 0000000..b90fa41 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/RaspiControl.csproj @@ -0,0 +1,20 @@ + + + + Exe + net6.0 + enable + enable + + + + + + + + + + + + +