Skip to content

Commit

Permalink
Fixup hal sim calls
Browse files Browse the repository at this point in the history
  • Loading branch information
@ThadHouse
ThadHouse committed Mar 10, 2024
1 parent a527fe6 commit 01d7ec4
Show file tree
Hide file tree
Showing 25 changed files with 653 additions and 612 deletions.
159 changes: 90 additions & 69 deletions dev/desktopDev/Program.cs
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,12 @@
using System;
using System.IO;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text.Json;
using CsCore;
using CsCore.Raw;
using UnitsNet.NumberExtensions.NumberToAngle;
using WPIHal.Natives.Simulation;
using WPIMath.Geometry;
using WPIUtil;
using WPIUtil.Natives;
Expand All @@ -17,79 +20,97 @@ class Program

static void Main(string[] args)
{
Rotation2d rot = new(5.Radians());
string serialized = JsonSerializer.Serialize(rot);
Console.WriteLine(serialized);

NativeLibrary.SetDllImportResolver(typeof(MJpegServer).Assembly, (name, assembly, searchpath) =>
{
if (name != "cscore")
{
throw new InvalidOperationException();
}
Console.WriteLine(searchpath);
return NativeLibrary.Load(@"C:\Users\thadh\Documents\GitHub\thadhouse\allwpilib\myRobot\build\install\myRobotCpp\windowsx86-64\lib\cscored.dll");
});

NativeLibrary.SetDllImportResolver(typeof(NativeRawFrame).Assembly, (name, assembly, searchpath) =>
{
if (name != "wpiutil")
{
throw new InvalidOperationException();
}
return NativeLibrary.Load(@"C:\Users\thadh\Documents\GitHub\thadhouse\allwpilib\myRobot\build\install\myRobotCpp\windowsx86-64\lib\wpiutild.dll");
});

Rotation2d r = JsonSerializer.Deserialize<Rotation2d>(serialized);
Console.WriteLine(r.Angle.Radians);

UsbCamera camera = new UsbCamera("Camera", 0)
{
ConnectionStrategy = ConnectionStrategy.ConnectionKeepOpen
};
MJpegServer server = new MJpegServer("Server", 1181)
{
Source = camera
};

RawSink sink = new RawSink("Sink")
{
Source = camera
};

RawSource source = new RawSource("Source", new VideoMode
{
Fps = 30,
Width = 640,
Height = 480,
PixelFormat = WPIUtil.PixelFormat.Yuyv
})
{
ConnectionStrategy = ConnectionStrategy.ConnectionKeepOpen
};

MJpegServer sinkServer = new MJpegServer("SinkServer", 1182)
{
Source = source
};

RawFrameReader reader = new RawFrameReader();

while (true)
{
long ts = sink.GrabFrame(reader);
if (ts <= 0)
{
continue;
var arr = typeof(HalAccelerometerData).Assembly.GetTypes().Where(x => x.Name.StartsWith("Hal") && x.Name.EndsWith("Data")).ToArray();

foreach (var item in arr) {
var nameWithoutHal = item.Name[3..];
var nameWithoutData = nameWithoutHal[..^4];
Console.WriteLine(nameWithoutData);
var file2 = $@"C:\Users\thadh\Documents\GitHub\robotdotnet\WPILib\src\hal\Natives\Simulation\{item.Name}.cs";
var txt = File.ReadAllText(file2);
foreach (var func in item.GetMethods()) {
var funcName = func.Name.Replace(nameWithoutData, "");

txt = txt.Replace($"{func.Name}(", $"{funcName}(");
//Console.WriteLine(func);
}

source.PutFrame(reader.ToWriter());
File.WriteAllText(file2, txt);
//Console.WriteLine(item);
}

Rotation2d rot = new(5.Radians());
string serialized = JsonSerializer.Serialize(rot);
Console.WriteLine(serialized);

// StructArrayTopic<TrackedTag> n = null!;
// var subscriber = n.Subscribe([], PubSubOptions.None);
// TrackedTag[] tags = subscriber.Get();
// NativeLibrary.SetDllImportResolver(typeof(MJpegServer).Assembly, (name, assembly, searchpath) =>
// {
// if (name != "cscore")
// {
// throw new InvalidOperationException();
// }
// Console.WriteLine(searchpath);
// return NativeLibrary.Load(@"C:\Users\thadh\Documents\GitHub\thadhouse\allwpilib\myRobot\build\install\myRobotCpp\windowsx86-64\lib\cscored.dll");
// });

// NativeLibrary.SetDllImportResolver(typeof(NativeRawFrame).Assembly, (name, assembly, searchpath) =>
// {
// if (name != "wpiutil")
// {
// throw new InvalidOperationException();
// }
// return NativeLibrary.Load(@"C:\Users\thadh\Documents\GitHub\thadhouse\allwpilib\myRobot\build\install\myRobotCpp\windowsx86-64\lib\wpiutild.dll");
// });

// Rotation2d r = JsonSerializer.Deserialize<Rotation2d>(serialized);
// Console.WriteLine(r.Angle.Radians);

// UsbCamera camera = new UsbCamera("Camera", 0)
// {
// ConnectionStrategy = ConnectionStrategy.ConnectionKeepOpen
// };
// MJpegServer server = new MJpegServer("Server", 1181)
// {
// Source = camera
// };

// RawSink sink = new RawSink("Sink")
// {
// Source = camera
// };

// RawSource source = new RawSource("Source", new VideoMode
// {
// Fps = 30,
// Width = 640,
// Height = 480,
// PixelFormat = WPIUtil.PixelFormat.Yuyv
// })
// {
// ConnectionStrategy = ConnectionStrategy.ConnectionKeepOpen
// };

// MJpegServer sinkServer = new MJpegServer("SinkServer", 1182)
// {
// Source = source
// };

// RawFrameReader reader = new RawFrameReader();

// while (true)
// {
// long ts = sink.GrabFrame(reader);
// if (ts <= 0)
// {
// continue;
// }

// source.PutFrame(reader.ToWriter());
// }


// // StructArrayTopic<TrackedTag> n = null!;
// // var subscriber = n.Subscribe([], PubSubOptions.None);
// // TrackedTag[] tags = subscriber.Get();
}
}

Expand Down
44 changes: 22 additions & 22 deletions ...l/Natives/Simulation/AccelerometerData.cs → ...atives/Simulation/HalAccelerometerData.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,91 +9,91 @@ public static unsafe partial class HalAccelerometerData
{
[LibraryImport("wpiHal", EntryPoint = "HALSIM_ResetAccelerometerData")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void ResetAccelerometerData(int index);
public static partial void ResetData(int index);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_RegisterAccelerometerActiveCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial int RegisterAccelerometerActiveCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);
public static partial int RegisterActiveCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_CancelAccelerometerActiveCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void CancelAccelerometerActiveCallback(int index, int uid);
public static partial void CancelActiveCallback(int index, int uid);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_GetAccelerometerActive")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
[return: MarshalAs(UnmanagedType.I4)]
public static partial bool GetAccelerometerActive(int index);
public static partial bool GetActive(int index);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_SetAccelerometerActive")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void SetAccelerometerActive(int index, [MarshalAs(UnmanagedType.I4)] bool active);
public static partial void SetActive(int index, [MarshalAs(UnmanagedType.I4)] bool active);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_RegisterAccelerometerRangeCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial int RegisterAccelerometerRangeCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);
public static partial int RegisterRangeCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_CancelAccelerometerRangeCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void CancelAccelerometerRangeCallback(int index, int uid);
public static partial void CancelRangeCallback(int index, int uid);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_GetAccelerometerRange")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial HalAccelerometer.Range GetAccelerometerRange(int index);
public static partial HalAccelerometer.Range GetRange(int index);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_SetAccelerometerRange")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void SetAccelerometerRange(int index, HalAccelerometer.Range range);
public static partial void SetRange(int index, HalAccelerometer.Range range);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_RegisterAccelerometerXCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial int RegisterAccelerometerXCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);
public static partial int RegisterXCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_CancelAccelerometerXCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void CancelAccelerometerXCallback(int index, int uid);
public static partial void CancelXCallback(int index, int uid);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_GetAccelerometerX")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial double GetAccelerometerX(int index);
public static partial double GetX(int index);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_SetAccelerometerX")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void SetAccelerometerX(int index, double x);
public static partial void SetX(int index, double x);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_RegisterAccelerometerYCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial int RegisterAccelerometerYCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);
public static partial int RegisterYCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_CancelAccelerometerYCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void CancelAccelerometerYCallback(int index, int uid);
public static partial void CancelYCallback(int index, int uid);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_GetAccelerometerY")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial double GetAccelerometerY(int index);
public static partial double GetY(int index);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_SetAccelerometerY")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void SetAccelerometerY(int index, double y);
public static partial void SetY(int index, double y);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_RegisterAccelerometerZCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial int RegisterAccelerometerZCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);
public static partial int RegisterZCallback(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_CancelAccelerometerZCallback")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void CancelAccelerometerZCallback(int index, int uid);
public static partial void CancelZCallback(int index, int uid);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_GetAccelerometerZ")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial double GetAccelerometerZ(int index);
public static partial double GetZ(int index);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_SetAccelerometerZ")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void SetAccelerometerZ(int index, double z);
public static partial void SetZ(int index, double z);

[LibraryImport("wpiHal", EntryPoint = "HALSIM_RegisterAccelerometerAllCallbacks")]
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
public static partial void RegisterAccelerometerAllCallbacks(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);
public static partial void RegisterAllCallbacks(int index, HAL_NotifyCallback callback, void* param, [MarshalAs(UnmanagedType.I4)] bool initialNotify);

}
Loading

0 comments on commit 01d7ec4

Please sign in to comment.