Example #1
0
        static void Main(string[] args)
        {
            LogEvents.Subscribe(i => Console.WriteLine(i.ToLogString()), new[]
            {
                typeof(PiTop4Board).Assembly,
                typeof(FoundationPlate).Assembly,
                typeof(ExpansionPlate).Assembly,
                typeof(RoverRobot).Assembly,
            });
            Console.WriteLine("Test Rover App");
            PiTop4Board.Instance.UseCamera();
            using var rover = new RoverRobot(PiTop4Board.Instance.GetOrCreateExpansionPlate(), PiTop4Board.Instance.GetOrCreateCamera <OpenCvCamera>(0), RoverRobotConfiguration.Default);

            var camControl   = rover.TiltController;
            var motorControl = rover.MotionComponent as SteeringMotorController;
            var js           = new LinuxJoystick();

            rover.AllLightsOn();
            rover.BlinkAllLights();

            Console.WriteLine("reset");

            while (!Console.KeyAvailable)
            {
                try
                {
                    var e = js.ReadEvent();
                    // Console.WriteLine($"ts={e.timestamp}, v={e.value}, t={e.type}, n={e.number}");
                    if (e.type == 1)
                    {
                        switch (e.number)
                        {
                        case 0:
                            if (e.value > 0)
                            {
                                rover.AllLightsOn();
                            }
                            else
                            {
                                rover.AllLightsOff();
                            }
                            break;
                        }
                    }
                    if (e.type == 2) // axis
                    {
                        switch (e.number)
                        {
                        case 0:     // steer
                            motorControl.Steering = RotationalSpeed.FromDegreesPerSecond(
                                e.value.Interpolate(-motorControl.MaxSteering.DegreesPerSecond,
                                                    motorControl.MaxSteering.DegreesPerSecond) / 2);

                            break;

                        case 1:     // throttle
                            motorControl.Speed = Speed.FromMetersPerSecond(
                                e.value.Interpolate(motorControl.MaxSpeed.MetersPerSecond,
                                                    -motorControl.MaxSpeed.MetersPerSecond) / 2);

                            break;


                        case 2:     // pan
                            camControl.Pan = Angle.FromDegrees(e.value.Interpolate(90, -90));
                            break;

                        case 3:     // tilt
                            camControl.Tilt = Angle.FromDegrees(
                                Math.Min(45, e.value.Interpolate(90, -90)));
                            break;
                        }
                    }
                }
                catch
                {
                    motorControl.Stop();
                    throw;
                }
            }

            Console.ReadKey();
            rover.AllLightsOff();

            Console.WriteLine("bye");
        }
Example #2
0
        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();
        }
Example #3
0
        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,
            //});

            ImageClassifier.Register("onnx", () => new OnnxImageClassifier());
            ImageClassifier classifier = null;
            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.B && e.Pressed)
            .Subscribe(e =>
            {
                rover.Camera.GetFrame().Save("/home/pi/shot.jpg");
            });

            js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.Y && e.Pressed)
            .Subscribe(e =>
            {
                classifier?.Dispose();
                var signatureFile = new FileInfo("/home/pi/models/pics/signature.json");
                classifier        = ImageClassifier.CreateFromSignatureFile(signatureFile);
                Console.WriteLine($"Loaded model from {signatureFile.FullName}");
            });

            js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.X && e.Pressed)
            .Subscribe(e =>
            {
                var frame  = rover.Camera.GetFrame().Focus();
                var result = classifier?.Classify(frame.CloneAs <Rgb24>());
                if (result is { })
                {
                    Console.WriteLine($"{result.Prediction.Label}");
                }
            });