private static async Task ConfigureRover(CSharpKernel csharpKernel) { Microsoft.DotNet.Interactive.Formatting.Formatter.ListExpansionLimit = 42; using var _ = Log.OnEnterAndExit(); await LoadAssemblyAndAddNamespace <RoverRobot>(csharpKernel); await LoadAssemblyAndAddNamespace <ResourceScanner>(csharpKernel); await AddNamespace(csharpKernel, typeof(ImageProcessing.ImageExtensions)); var RoverBody = new RoverRobot(PiTop4Board.Instance.GetOrCreateExpansionPlate(), PiTop4Board.Instance.GetOrCreateCamera <StreamingCamera>(0), RoverRobotConfiguration.Default); var RoverBrain = new RoverRobotAgent(); var ResourceScanner = new ResourceScanner(); RoverBody.BlinkAllLights(); await csharpKernel.SetVariableAsync(nameof(RoverBody), RoverBody); await csharpKernel.SetVariableAsync(nameof(RoverBrain), RoverBrain); await csharpKernel.SetVariableAsync(nameof(ResourceScanner), ResourceScanner); var command = new Command("#!reset", "Reset RoverBody, RoverBrain and BrainState") { new Option <bool>("--body", description: "Resets the rover body"), new Option <bool>("--brain", description: "Resets the rover brain"), new Option <bool>("--state", description: "Resets the rover brain state"), new Option <bool>("--all", description: "Resets the entire rover"), }; command.Handler = CommandHandler.Create <bool, bool, bool, bool, KernelInvocationContext>(async(body, brain, state, all, context) => { if (body || brain || state || all) { var code = new StringBuilder(); var resetTarget = new List <string>(); if (brain || all) { code.AppendLine($"{nameof(RoverBrain)}.Reset();"); resetTarget.Add("brain"); } if (state || all) { code.AppendLine($"{nameof(RoverBrain)}.ClearState();"); resetTarget.Add("state"); } if (body || all) { code.AppendLine($"{nameof(RoverBody)}.Reset();"); resetTarget.Add("body"); } var value = context.Display($"Reset for {string.Join(", ", resetTarget)} in progress", PlainTextFormatter.MimeType); await csharpKernel.SendAsync(new SubmitCode(code.ToString())); value.Update($"Reset for {string.Join(", ", resetTarget)} done!"); } }); csharpKernel.AddDirective(command); var source = new CancellationTokenSource(); var robotLoop = Task.Run(() => { using var operation = Log.OnEnterAndExit("roverBrainLoop"); while (!source.IsCancellationRequested) { if (!source.IsCancellationRequested) { using var __ = operation.OnEnterAndExit("Perceive"); try { RoverBrain.Perceive(); } catch (Exception e) { __.Error(e); } } if (!source.IsCancellationRequested) { var planResult = PlanningResult.NoPlan; using var ___ = operation.OnEnterAndExit("Plan"); try { planResult = RoverBrain.Plan(); } catch (Exception e) { ___.Error(e); planResult = PlanningResult.NoPlan; } if (!source.IsCancellationRequested && planResult != PlanningResult.NoPlan) { using var ____ = operation.OnEnterAndExit("Act"); RoverBrain.Act(); } } } RoverBody.MotionComponent.Stop(); }, source.Token); var reactLoop = Task.Run(() => { using var operation = Log.OnEnterAndExit("roverBrainReactLoop"); while (!source.IsCancellationRequested) { if (!source.IsCancellationRequested) { using var __ = operation.OnEnterAndExit("React"); try { RoverBrain.React(); } catch (Exception e) { __.Error(e); } } } RoverBody.MotionComponent.Stop(); }, source.Token); csharpKernel.RegisterForDisposal(() => { source.Cancel(false); Task.WaitAll(new[] { robotLoop, reactLoop }, TimeSpan.FromSeconds(10)); RoverBody.Dispose(); }); }
static void Main(string[] args) { LogEvents.Subscribe(i => { i.Operation.Id = ""; Console.WriteLine(i.ToLogString()); }, new[] { typeof(PiTop4Board).Assembly, typeof(FoundationPlate).Assembly, typeof(ExpansionPlate).Assembly, typeof(RoverRobot).Assembly, typeof(StreamingCamera).Assembly, typeof(Program).Assembly, }); var js = new XBoxController(); // using ` mjpg_streamer -i "input_uvc.so -d /dev/video0" -o output_http.so` // ==> http://pi-top.local:8080/?action=stream PiTop4Board.Instance.UseCamera(); using var rover = new RoverRobot(PiTop4Board.Instance.GetOrCreateExpansionPlate(), PiTop4Board.Instance.GetOrCreateCamera <StreamingCamera>(0), RoverRobotConfiguration.Default); var camControl = rover.TiltController; var motorControl = rover.MotionComponent as SteeringMotorController; rover.AllLightsOn(); rover.BlinkAllLights(); Observable.Interval(TimeSpan.FromMilliseconds(10)) .Select(_ => (X: js.LeftStick.X, Y: js.LeftStick.Y)) .DistinctUntilChanged() .Subscribe(stick => { var left = stick.X.WithDeadZone(-.5, .5, .3); var forward = stick.Y; motorControl.SetPower((forward + left) / 1.5, (forward - left) / 1.5); }); js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.A) .Subscribe(e => { if (e.Pressed) { rover.AllLightsOn(); } else { rover.AllLightsOff(); } }); js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.X && e.Pressed) .Subscribe(e => { rover.Camera.GetFrame().Save("/home/pi/shot.jpg"); }); Observable.Interval(TimeSpan.FromMilliseconds(100)) .Select(_ => (X: js.RightStick.X, Y: js.RightStick.Y)) .DistinctUntilChanged() .Subscribe(stick => { camControl.SetSpeeds( RotationalSpeed.FromRadiansPerSecond(stick.X / 3), RotationalSpeed.FromRadiansPerSecond(stick.Y / 3) ); }); js.Events.OfType <ButtonEvent>().Subscribe(e => Console.WriteLine($"Button: {e.Button} {e.Pressed}")); js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.RightStick) .Subscribe(e => { camControl.Reset(); }); //Observable.Interval(TimeSpan.FromMilliseconds(100)) // .Select(_ => // new Unit[10].Select(_ => rover.UltrasoundFront.Distance.Centimeters).ToArray()) // .Subscribe(l => // { // var mean = l.Average(); // var stddev = Math.Sqrt(l.Select(d => (d - mean) * (d - mean)).Average()); // var maxrange = 1.5 * stddev; // var valid = l.Where(d => Math.Abs(d - mean) < maxrange).ToList(); // if (valid.Count > 0) // { // Console.WriteLine($"Distance= {valid.Average():F1} cm ({valid.Count}: mean {mean:F1}, stddev {stddev:F1}, max {valid.Max():F1}, min {valid.Min():F1})"); // } // }); var oef = new OneEuroFilter(minCutoff: 0.004, beta: 0.7); Observable.Interval(TimeSpan.FromMilliseconds(100)).Select(_ => oef.Apply(rover.UltrasoundFront.Distance.Centimeters)) .Scan(new List <double>(), (list, d) => // sliding window { const int window = 10; list.Add(d); if (list.Count > window) { list.RemoveRange(0, list.Count - window); } return(new List <double>(list)); }) .Subscribe(l => { var mean = l.Average(); var stddev = Math.Sqrt(l.Select(d => (d - mean) * (d - mean)).Average()); var maxrange = 1.5 * stddev; var valid = l.Where(d => Math.Abs(d - mean) < maxrange).ToList(); if (valid.Count > 0) { Console.WriteLine($"Distance= {valid.Average():F1} cm ({valid.Count}: mean {mean:F1}, stddev {stddev:F1}, max {valid.Max():F1}, min {valid.Min():F1})"); } }); Console.WriteLine("Ok, go drive around"); Console.ReadKey(); rover.AllLightsOff(); rover.Dispose(); }
private static async Task ConfigureRover(CSharpKernel csharpKernel) { using var _ = Log.OnEnterAndExit(); await LoadAssemblyAndAddNamespace <RoverRobot>(csharpKernel); await LoadAssemblyAndAddNamespace <ResourceScanner>(csharpKernel); await AddNamespace(csharpKernel, typeof(ImageProcessing.ImageExtensions)); var roverBody = new RoverRobot(PiTop4Board.Instance.GetOrCreateExpansionPlate(), PiTop4Board.Instance.GetOrCreateCamera <StreamingCamera>(0), RoverRobotConfiguration.Default); var roverBrain = new RoverRobotStrategies(); var resourceScanner = new ResourceScanner(); roverBody.BlinkAllLights(); await csharpKernel.SetVariableAsync(nameof(roverBody), roverBody); await csharpKernel.SetVariableAsync(nameof(roverBrain), roverBrain); await csharpKernel.SetVariableAsync(nameof(resourceScanner), resourceScanner); var source = new CancellationTokenSource(); var robotLoop = Task.Run(() => { using var operation = Log.OnEnterAndExit("roverBrainLoop"); while (!source.IsCancellationRequested) { var localCancellationSource = new CancellationTokenSource(); if (!source.IsCancellationRequested && !localCancellationSource.IsCancellationRequested) { using var __ = operation.OnEnterAndExit("Perceive"); try { roverBrain.Perceive?.Invoke(roverBody, DateTime.Now, localCancellationSource.Token); } catch (Exception e) { __.Error(e); } } if (!source.IsCancellationRequested && !localCancellationSource.IsCancellationRequested) { var planResult = PlanningResult.NoPlan; using var ___ = operation.OnEnterAndExit("Plan"); try { planResult = roverBrain.Plan?.Invoke(roverBody, DateTime.Now, localCancellationSource.Token) ?? PlanningResult.NoPlan; } catch (Exception e) { ___.Error(e); planResult = PlanningResult.NoPlan; } if (!source.IsCancellationRequested && planResult != PlanningResult.NoPlan && !localCancellationSource.IsCancellationRequested) { using var ____ = operation.OnEnterAndExit("Act"); roverBrain.Act?.Invoke(roverBody, DateTime.Now, localCancellationSource.Token); } } } roverBody.MotionComponent.Stop(); }, source.Token); var reactLoop = Task.Run(() => { using var operation = Log.OnEnterAndExit("roverBrainReactLoop"); while (!source.IsCancellationRequested) { var localCancellationSource = new CancellationTokenSource(); if (!source.IsCancellationRequested && !localCancellationSource.IsCancellationRequested) { using var __ = operation.OnEnterAndExit("React"); try { roverBrain.React?.Invoke(roverBody, DateTime.Now, localCancellationSource.Token); } catch (Exception e) { __.Error(e); } } } roverBody.MotionComponent.Stop(); }, source.Token); csharpKernel.RegisterForDisposal(() => { source.Cancel(false); Task.WaitAll(new[] { robotLoop, reactLoop }, TimeSpan.FromSeconds(10)); roverBody.Dispose(); }); }