From 10becababfefbd6c566b5aaa5cd5e696bc06c4f8 Mon Sep 17 00:00:00 2001
From: duc anh vu <vuducanh@informatik.hu-berlin.de>
Date: Fri, 20 Apr 2018 15:54:33 +0200
Subject: [PATCH] add sonar
---
.../src/car/include/ultrasonic/USS_SRF02.h | 21 +++++++
.../src/car/src/ultrasonic/USS_SRF02.cpp | 63 +++++++++++++++++++
2 files changed, 84 insertions(+)
create mode 100644 modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h
create mode 100644 modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h b/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h
new file mode 100644
index 00000000..092d475a
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h
@@ -0,0 +1,21 @@
+#ifndef USS_SRF02_H_
+#define USS_SRF02_H_
+
+class USS_SRF02{
+
+public:
+ USS_SRF02(int devId);
+
+ int getDistance();
+
+private:
+
+ void setup();
+
+ int devId;
+
+ int fd;
+};
+
+
+#endif /* USS_SRF02_H_ */
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
new file mode 100644
index 00000000..85243aad
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
@@ -0,0 +1,63 @@
+#include "../include/USS_SRF02.h"
+#include "../lib/wiringPi/wiringPi/wiringPiI2C.h"
+
+#include <iostream>
+
+#include <unistd.h>
+
+//gpio pins where the sonar is connected
+const int SRF02_SDA = 8; //i2c data pin ;
+const int SRF02_SCL = 9; //i2c clock pin ;
+
+// addresses of the sonars, the number is also as a sticker on the devices themselves
+const int DEVICE_ADDRESS1 = 0x74;
+const int DEVICE_ADDRESS2 = 0x76;
+const int DEVICE_ADDRESS3 = 0x77;
+
+//path to i2c file
+const char *DEVICE = "/dev/i2c-1";
+
+const int COMMAND_REGISTER = 0x00;
+const int RESULT_HIGH_BYTE = 0x02;
+const int RESULT_LOW_BYTE = 0x03;
+const int RANGING_MODE_CM = 0x51;
+
+const int DELAY = 70; //70 ms for ranging to finish
+
+int main() {
+
+ USS_SRF02 uss(0x74);
+
+ while(1){
+ std::cout << uss.getDistance() << std::endl;
+ }
+ return 0;
+}
+
+USS_SRF02::USS_SRF02(int devId) {
+ this->fd = -1; //no file opened yet
+ this->devId = devId;
+}
+
+int USS_SRF02::getDistance() {
+ int distance;
+
+ if(fd == -1){
+ USS_SRF02::setup();
+ }
+
+ wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
+ usleep(DELAY * 1000);
+ distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+
+ return distance;
+}
+
+void USS_SRF02::setup() {
+
+ USS_SRF02::fd = wiringPiI2CSetupInterface(DEVICE, this->devId);
+ //todo error handling,
+ //however wiringPiI2CSetupInterface() calls exit() if it fails
+
+
+}
--
GitLab