Over the last few days I've been knocking together a little class to run an HC-SR04 ultrasonic sensor, although I'm sure other brands/models would work too.
It works awesome except that when I add a second sensor the code just follows the second sensor and ignores the first.
Can anyone help?
/* UltraSonic Sensor Helper * * Ultrasonic Sensor Interface, based on the HC-SR4 Ultrasonic Sensor * Returns a range 200 - 400mm * * Tested on Netduino 1 * * Connections: * Netduino UltraSonic Sensor * Digital Pin Trigger * Digital Pin Echo * 5V DC VCC (3.3V DC works awesome as well) * GND GND * */using System;using System.Threading;using Microsoft.SPOT;using Microsoft.SPOT.Hardware;using SecretLabs.NETMF.Hardware;using SecretLabs.NETMF.Hardware.Netduino;namespace SensorHelper{ /// <summary> /// Ultrasonic Sensor Interface. Returns a range 200 - 400 mm. Based on the HC-SR4. /// </summary> public class UltraSonic { // initialise private static InterruptPort EchoPulse; private static Cpu.Pin trigPin; private static Cpu.Pin echoPin; private static long distance; // in mm private static long pulseWidthInTicks; // ticks are microseconds / 10 private static long ticksCurrent; // Current and Previous used to determine the pulse width private static long ticksPrevious; // public UltraSonic(Cpu.Pin trigger, Cpu.Pin echo) { trigPin = trigger; // Set digital pins for this particular sensor echoPin = echo; ListenForEcho(echoPin); // Start interrupt listening on echo pin } /// <summary> /// Find the distance to an object from 200 - 400mm in range /// </summary> /// <returns>Range to object in mm</returns> public int GetDist() { pingTriggerPin(trigPin); // Send a short pulse on the digital pin return (int)distance; // the distance in mm } /// <summary> /// Sends a short ping to make the sensor search for an object /// </summary> /// <param name="trigPin">Digital pin connected to the sensors trigger pin</param> private static void pingTriggerPin(Cpu.Pin trigPin) { // Send a short ping pulse on trigger pin OutputPort pulseOut = new OutputPort(trigPin, false); pulseOut.Write(true); // Start ping //Thread.Sleep(1); // Ping length of 1ms, 5ms and no sleep (all work the same) // (data sheet recommends ping of 10 mircoseconds / 0.01ms) pulseOut.Write(false); // Stop ping pulseOut.Dispose(); } /// <summary> /// Starts an interrupt on a digital pin to listen for the echo pulse from sensor /// </summary> /// <param name="echoPin">Digital pin connected the the sensors echo pin</param> private static void ListenForEcho(Cpu.Pin echoPin) { // Listen for echo/pulseIn EchoPulse = new InterruptPort(echoPin, false, Port.ResistorMode.Disabled, Port.InterruptMode.InterruptEdgeBoth); EchoPulse.OnInterrupt += EchoPulse_OnInterrupt; // Wait for echo response } /// <summary> /// Interrupt for determining the length on the echo pulse /// </summary> /// <param name="data1">No sure what data1 is exactly</param> /// <param name="state">Rising or Faliing edge of echo pulse</param> /// <param name="time">Current time</param> private static void EchoPulse_OnInterrupt(uint data1, uint state, DateTime time) { // Falling edge - end of echo pulse if (state == 0) { ticksCurrent = time.Ticks; pulseWidthInTicks = ticksCurrent - ticksPrevious; // This is where the echo pulse duration is determined long pulseWidthMicroseconds = pulseWidthInTicks / 10; // Convert to microseconds if (pulseWidthMicroseconds < 250) // Test for jitter { return; } else { // The speed of sound is 340 m/s or 3400cm/ms or 3.4cm/us // Coverting to microseconds per centimetre (1 / 0.034) gives ~29 us/cm. // The sound ping travels out, bounces off and object and returns back to the sensor, // so to find the distance of the object we take half of the distance travelled. distance = (pulseWidthMicroseconds / 3) / 2; } } // Rising edge - start of echo pulse else { ticksPrevious = time.Ticks; } } }}
I'm using it like this...
while (true) { int ping1 = ultraSonic1.GetDist(); // Range 200 - 400 mm int ping2 = ultraSonic2.GetDist(); Debug.Print("Sensor 1: " + ping1.ToString() + "ttSensor 2: " + ping2.ToString()); toolBox.Pause(500); }