| /* |
| * Copyright (c) 2017 FH Dortmund and others |
| * All rights reserved. This program and the accompanying materials |
| * are made available under the terms of the Eclipse Public License v1.0 |
| * which accompanies this distribution, and is available at |
| * http://www.eclipse.org/legal/epl-v10.html |
| * |
| * Description: |
| * RoverHCSR04 API - Interfaces for Rover ultrasonic sensor application development |
| * |
| * Contributors: |
| * M.Ozcelikors <mozcelikors@gmail.com>, created RoverHCSR04 class 04.12.2017 |
| * |
| */ |
| |
| #include <roverapi/rover_hcsr04.hpp> |
| #include <wiringPi.h> |
| #include <stdint.h> |
| #include <stdio.h> |
| #include <unistd.h> |
| |
| rover::RoverHCSR04::RoverHCSR04 (const RoverSensorID_t sensor_id) |
| :ROVERHCSR04_SETUP_(0) |
| { |
| this->sensorID = sensor_id; |
| if (this->sensorID == ROVER_FRONT) |
| { |
| this->trigPin = TRIG1; |
| this->echoPin = ECHO1; |
| } |
| else if (this->sensorID == ROVER_REAR) |
| { |
| this->trigPin = TRIG0; |
| this->echoPin = ECHO0; |
| } |
| else |
| { |
| printf ("Invalid Sensor ID for the Ultrasonic Sensor to constructor RoverHCSR04::RoverHCSR04().\n"); |
| // Adding a default value for the pins in case there is an error |
| this->trigPin = TRIG0; |
| this->echoPin = ECHO0; |
| } |
| } |
| |
| rover::RoverHCSR04::~RoverHCSR04(){} |
| |
| void rover::RoverHCSR04::initialize (void) |
| { |
| pinMode(this->trigPin, OUTPUT); |
| pinMode(this->echoPin, INPUT); |
| |
| //TRIG pin must start LOW |
| digitalWrite(this->trigPin, LOW); |
| delayMicroseconds(2); |
| |
| this->ROVERHCSR04_SETUP_ = 1; |
| } |
| |
| float rover::RoverHCSR04::read (void) |
| { |
| if (this->ROVERHCSR04_SETUP_ != 1) |
| { |
| fprintf(stderr,"You havent set up RoverHCSR04. Use RoverHCSR04()::initialize() !\n"); |
| } |
| else |
| { |
| int distance = 0; |
| //Send trig pulse |
| digitalWrite(this->trigPin, HIGH); |
| delayMicroseconds(10); |
| digitalWrite(this->trigPin, LOW); |
| |
| //Wait for echo start |
| long startTime = micros(); |
| while(digitalRead(this->echoPin) == LOW && micros() < startTime + 100000); |
| |
| //Wait for echo end |
| startTime = micros(); |
| while(digitalRead(this->echoPin) == HIGH); |
| long travelTime = micros() - startTime; |
| |
| //Get distance in cm |
| distance = travelTime * 34300; |
| distance = distance / 1000000; |
| distance = distance / 2; |
| // The below protection is to ensure there is no value fluctuation due to timeout |
| if (distance > 40 ) |
| distance = 40; |
| |
| // printf("dist=%d\n",distance); |
| return ((float) distance*1.0); |
| } |
| } |
| |
| void rover::RoverHCSR04::setTrigPin (const int trig_pin) |
| { |
| this->trigPin = trig_pin; |
| } |
| |
| int rover::RoverHCSR04::getTrigPin (void) |
| { |
| return this->trigPin; |
| } |
| |
| void rover::RoverHCSR04::setEchoPin (const int echo_pin) |
| { |
| this->echoPin = echo_pin; |
| } |
| |
| int rover::RoverHCSR04::getEchoPin (void) |
| { |
| return this->echoPin; |
| } |
| |