I have a raspberry Pi Zero with the AlphaBot2 which has the HC-SR04 ultrasonic sensor. The implementation using Python works well. I want to implement in C because I need to bind it with another program also in C and for optimization reasons. The first thing that I noticed is the Python code uses the RPi.GPIO lib and in C I have to use wiringPi or bcm2835. So I decided to use the wiringPi lib. My program executes but the distance is not correct. One thing that differs from the implementations that I found at web is that I am using TRIG 22 and ECHO 27, because my HC-SR04 is connected at the AlphaBot2. I am not using 2 resistors to connect it to the raspberry Pi. When I put some barrier in front of the sensor I get only 3 and 5 centimeters even if I move it to 30 centimeters.
Distance: 3905724cm
Distance: 5cm
Distance: 5cm
Distance: 5cm
Distance: 5cm
Distance: 3cm
Distance: 3cm
Distance: 5cm
Distance: 5cm
Here is my code:
#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>
#include "ultrasonicClient.h"
#define TRUE (1==1)
// HC-SR04 ultrasonic sensor on AlphaBot2 Pi Zero
#define TRIG 22
#define ECHO 27
static volatile long startTimeUsec;
static volatile long endTimeUsec;
double speedOfSoundMetersPerSecond = 340.29;
void recordPulseLength() {
startTimeUsec = micros();
while (digitalRead(ECHO) == HIGH);
endTimeUsec = micros();
}
void setupUltrasonic() {
wiringPiSetup();
pinMode(TRIG, OUTPUT);
pinMode(ECHO, INPUT);
// TRIG pin must start LOW
// Initialize the sensor's trigger pin to low. If we don't pause
// after setting it to low, sometimes the sensor doesn't work right.
digitalWrite(TRIG, LOW);
delay(500); // .5 seconds
}
int getCM() {
// Send trig pulse
// Triggering the sensor for 10 microseconds will cause it to send out
// 8 ultrasonic (40Khz) bursts and listen for the echos.
digitalWrite(TRIG, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG, LOW);
int now = micros();
// Wait for echo start
// The sensor will raise the echo pin high for the length of time that it took
// the ultrasonic bursts to travel round trip.
while (digitalRead(ECHO) == LOW && micros() - now < 30000);
recordPulseLength();
long travelTimeUsec = endTimeUsec - startTimeUsec;
double distanceMeters = 100 * ((travelTimeUsec / 1000000.0) * 340.29) / 2;
//Wait for echo end
long startTime = micros();
while (digitalRead(ECHO) == HIGH);
long travelTime = micros() - startTime;
//Get distance in cm
int distance = travelTime * 34000 / 2;
return distanceMeters * 100;
}
int runUltrasonicClient() {
int count = 0;
setupUltrasonic();
while (count < 60) {
printf("Distance: %dcm\n", getCM());
count++;
delay(500); // 0.5 second
}
return 0;
}
I found a code to execute using bcm2835.
static uint64_t cyclePulse(int trigger, int echo) {
if (!bcm2835_init())
return 1;
// Set RPi pin echo to be an input pin
bcm2835_gpio_fsel(echo, BCM2835_GPIO_FSEL_INPT);
// Set RPi pin P1-11 to be an output pin
bcm2835_gpio_fsel(trigger, BCM2835_GPIO_FSEL_OUTP);
// Declare the unsigned int timer variables to measure pulses
uint64_t width, begin, start, end;
int max = 80, check;
begin = bcm2835_st_read();
// Emit pulse for 10 microseconds
bcm2835_gpio_write(trigger, HIGH); // Set trigger state HIGH
bcm2835_delayMicroseconds(10); // Wait 10 microseconds
bcm2835_gpio_write(trigger, LOW); // Set trigger state LOW
// Infinite loop until a pulse is received
while (bcm2835_gpio_lev(echo) == LOW && check < max) {
start = bcm2835_st_read();
check = (int) begin - start;
}
// Loop and delay for one microsecond until falling edge detected
while (bcm2835_gpio_lev(echo) == HIGH) {
bcm2835_delayMicroseconds(1);
}
// Record the ending time of the pulse to get the pulse width
end = bcm2835_st_read();
// Get the final with of the pulse
width = end - start;
//Close the bcm2835 bridge
bcm2835_close();
// Return the total width of the returned pulse
return width;
}