add sonar

modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h

+#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_ */

modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp

+#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
+
+
+}