public static void Run () { var scope = new DebugScope (); // // Create the robot // var leftMotor = HBridgeMotor.CreateForNetduino (PWMChannels.PWM_PIN_D3, Pins.GPIO_PIN_D1, Pins.GPIO_PIN_D2); leftMotor.CalibrationInput.Value = 1; var rightMotor = HBridgeMotor.CreateForNetduino (PWMChannels.PWM_PIN_D6, Pins.GPIO_PIN_D4, Pins.GPIO_PIN_D5); rightMotor.CalibrationInput.Value = 1; // // Create his eyes // var leftRange = new SharpGP2D12 { Name = "LeftRange" }; var rightRange = new SharpGP2D12 { Name = "RightRange" }; leftRange.AnalogInput.ConnectTo (new AnalogInputPin (AnalogChannels.ANALOG_PIN_A0, 10).Analog); rightRange.AnalogInput.ConnectTo (new AnalogInputPin (AnalogChannels.ANALOG_PIN_A1, 10).Analog); scope.ConnectTo (leftRange.DistanceOutput); scope.ConnectTo (rightRange.DistanceOutput); // // Now some intelligence // Each motor is driven by the distance sensor's reading // var nearDist = 0.1; var farDist = 0.5; var minSpeed = 0.4; TransformFunction distanceToSpeed = d => { if (d < nearDist) { return -minSpeed; } if (d > farDist) { return 1; } var a = (d - nearDist) / (farDist - nearDist); return a * (1 - minSpeed) + minSpeed; }; var leftSpeed = new Transform (distanceToSpeed); leftSpeed.Input.ConnectTo (leftRange.DistanceOutput); leftSpeed.Output.ConnectTo (leftMotor.SpeedInput); var rightSpeed = new Transform (distanceToSpeed); rightSpeed.Input.ConnectTo (rightRange.DistanceOutput); rightSpeed.Output.ConnectTo (rightMotor.SpeedInput); }
public static void Run () { var scope = new DebugScope (); //scope.UpdatePeriod.Value = 1; var accel = new Memsic2125 (); accel.XPwmInput.ConnectTo (new DigitalInputPin (Pins.GPIO_PIN_D6).Output); accel.YPwmInput.ConnectTo (new DigitalInputPin (Pins.GPIO_PIN_D7).Output); scope.ConnectTo (accel.XAccelerationOutput); scope.ConnectTo (accel.YAccelerationOutput); var compass = new Grove3AxisDigitalCompass (); scope.ConnectTo (compass.XGaussOutput); scope.ConnectTo (compass.YGaussOutput); scope.ConnectTo (compass.ZGaussOutput); var a0 = new AnalogInputPin (AnalogChannels.ANALOG_PIN_A0); scope.ConnectTo (a0.Analog); var sharp = new SharpGP2D12 (); a0.Analog.ConnectTo (sharp.AnalogInput); scope.ConnectTo (sharp.DistanceOutput); var therm = new Thermistor (); therm.AnalogInput.ConnectTo (a0.Analog); scope.ConnectTo (therm.Temperature); var b = new CelsiusToFahrenheit (); therm.Temperature.ConnectTo (b.Celsius); scope.ConnectTo (b.Fahrenheit); var bmp = new Bmp085 (); scope.ConnectTo (bmp.Temperature); var b2 = new CelsiusToFahrenheit (); bmp.Temperature.ConnectTo (b2.Celsius); scope.ConnectTo (b2.Fahrenheit); for (; ; ) { Debug.Print ("Tick"); Thread.Sleep (1000); } }
public static void Run () { // // Start with the basic robot // var leftMotor = HBridgeMotor.CreateForNetduino (PWMChannels.PWM_PIN_D3, Pins.GPIO_PIN_D1, Pins.GPIO_PIN_D2); var rightMotor = HBridgeMotor.CreateForNetduino (PWMChannels.PWM_PIN_D6, Pins.GPIO_PIN_D4, Pins.GPIO_PIN_D5); var robot = new TwoWheeledRobot (leftMotor, rightMotor); // // Create a range finder and scope it // var scope = new DebugScope (); var a0 = new AnalogInputPin (AnalogChannels.ANALOG_PIN_A0, 10); scope.ConnectTo (a0.Analog); var sharp = new SharpGP2D12 (); a0.Analog.ConnectTo (sharp.AnalogInput); scope.ConnectTo (sharp.DistanceOutput); scope.ConnectTo (robot.SpeedInput); // // This is the cruising (unobstructed) speed // var distanceToSpeed = new Transform (distance => { const double min = 0.1; const double max = 0.5; if (distance > max) { return 1.0; } else if (distance < min) { return 0.0; } return (distance - min) / (max - min); }); distanceToSpeed.Input.ConnectTo (sharp.DistanceOutput); // // Take different actions depending on our environment: // 0: cruising // 1: collided // var sw = new Switch ( new [] { new Connection (distanceToSpeed.Output, robot.SpeedInput), new Connection (new SineWave (0.5, 0.333, 0, updateFrequency: 10).Output, robot.DirectionInput), new Connection (new Constant (0).Output, robot.SpinInput), }, new[] { new Connection (new Constant (0.6).Output, robot.SpinInput), new Connection (new Constant (0).Output, robot.DirectionInput), new Connection (new Constant (0).Output, robot.SpeedInput), }); var collided = new Transform (distance => distance < 0.2 ? 1 : 0); collided.Input.ConnectTo (sharp.DistanceOutput); collided.Output.ConnectTo (sw.Input); // // Loop to keep us alive // for (; ; ) { //Debug.Print ("TwoWheeled Tick"); Thread.Sleep (1000); } }