int main(int argc,char *argv[])
{
int fd, res, i, del;
unsigned char st, ver;
// First arg is delay in ms (default is 1000)
if (argc > 1)
del = atoi(argv[1]);
else del=1000;
fd = lidar_init(false);
if (fd == -1) {
printf("initialization error\n");
}
else {
for (i=0;i<10000;i++) {
res = lidar_read(fd);
st = lidar_status(fd);
//ver = lidar_version(fd);
printf("%3.0d cm \n", res);
//lidar_status_print(st);
//delay(del);
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "lidarlite_node");
ros::NodeHandle n;
ros::Publisher lidar_pub = n.advertise<lidar_lite_ros::Lidarlite>("lidar_distance", 1);
ros::Rate loop_rate(20);
int fd;
int value;
unsigned char status;
lidar_lite_ros::Lidarlite msg;
fd = lidar_init(false);
if (fd == -1) {
printf("initialization error\n");
}
while (ros::ok())
{
value = lidar_read(fd);
status = lidar_status(fd);
ROS_INFO("%3.0d cm", value);
msg.distance = value;
lidar_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
// Initialize wiring I2C interface to LidarLite
int lidar_init(bool dbg) {
int fd;
_dbg = dbg;
if (_dbg) printf("LidarLite V0.1\n\n");
fd = wiringPiI2CSetup(LIDAR_LITE_ADRS);
if (fd != -1) {
lidar_status(fd); // Dummy request to wake up device
delay (100);
}
return(fd);
}
// Initialize wiring I2C interface to LidarLite
int lidarLite::lidar_init(bool dbg) {
int fd;
m_dbg=dbg;
if (m_dbg) printf("LidarLite V0.1\n\n");
fd = wiringPiI2CSetup(m_addr);
if (fd != -1) {
lidar_status(fd); // Dummy request to wake up device
usleep (100000);
}
return(fd);
}
int main(int argc,char *argv[])
{
int fd, res, i, del;
unsigned char st, ver;
const double cm_to_inch = 2.54; // (0.39);
// First arg is delay in ms (default is 1000)
if (argc > 1)
del = atoi(argv[1]);
else del=1000;
float cm, inches;
int feet;
fd = lidar_init(false);
if (fd == -1) {
printf("initialization error\n");
}
else {
res = lidar_read(fd);
st = lidar_status(fd);
//ver = lidar_version(fd);
cm = (float)res;
inches = (float)res / cm_to_inch;
feet = inches/12;
//inches = inches-(feet*12);
//printf("%.1f cm = %d feet, %.1f inches\n", cm, feet, inches);
printf("%3.0d \n", res);
//lidar_status_print(st);
//delay(del);
}
}
//--------------------------------------------------------------
void ofApp::draw(){
ofSleepMillis(100);
int newVal = lidar_read(fd);
unsigned char st = lidar_status(fd);
//ver = lidar_version(fd);
//printf("%3.0d cm \n", res);
lidar_status_print(st);
if (newVal < 2) {
// Handle strange case where lidar reports 1cm when should be infinity
newVal = 10000;
}
// increment up to max smoothing (deals with initial state)
if (nSamplesToSmooth < maxSamplesToSmooth ) nSamplesToSmooth++;
// Calculate the weight of new value
float newWeight = ((float)1)/((float)nSamplesToSmooth);
// Calculate the smoothed PWM value
smoothPwm = ((float) newVal)*newWeight + smoothPwm*(1-newWeight);
counter++;
if (pitchBend) { // Pitch bend mode
//cout << ofGetSystemTimeMicros() << "," << blinkTimer << endl;
if (ofGetSystemTimeMicros() - blinkTimer >= 200000) {
// Blink the LED
if (gpio15outState) {
gpio15->setval_gpio("0");
gpio15outState = false;
} else {
gpio15->setval_gpio("1");
gpio15outState = true;
}
if (gpio21outState) {
gpio21->setval_gpio("0");
gpio21outState = false;
} else {
gpio21->setval_gpio("1");
gpio21outState = true;
}
// Play sound
pitchSound.play();
float soundSpeed = 1.5f - ofMap(smoothPwm, minDist, maxDist, 0.f, 1.f, true);
pitchSound.setSpeed( soundSpeed );
//ofSoundSetVolume(ofClamp(0.5f + smoothPwm, 0, 1));
cout << getDateTimeString() << ", " << counter << " loops, " << ofGetFrameRate()
<< "Hz , " << smoothPwm << " cm" << " , " << soundSpeed <<", " << (int) st;
if (gpio15outState){
cout << ",LED=ON";
} else {
cout << ",LED=OFF";
}
cout << endl;
blinkTimer = ofGetSystemTimeMicros();
counter = 0;
}
} else if (volBend) { // Volume bend mode
float soundVolume = 1.f - ofMap(smoothPwm, minDist, maxDist, 0.f, 1.f, true);
if (ofGetSystemTimeMicros() - blinkTimer >= 1000000) {
volSound.play();
// Blink the LED
if (gpio15outState) {
gpio15->setval_gpio("0");
gpio15outState = false;
} else {
gpio15->setval_gpio("1");
gpio15outState = true;
}
if (gpio21outState) {
gpio21->setval_gpio("0");
gpio21outState = false;
} else {
gpio21->setval_gpio("1");
gpio21outState = true;
}
cout << getDateTimeString() << ", " << counter << " loops, " << ofGetFrameRate()
<< "Hz , " << smoothPwm << " cm" << " , " << soundVolume <<", " << (int) st;
if (gpio15outState){
cout << ",LED=ON";
} else {
cout << ",LED=OFF";
}
cout << endl;
blinkTimer = ofGetSystemTimeMicros();
counter = 0;
}
volSound.setVolume(soundVolume);
ofSoundSetVolume(soundVolume);
}
// Log data to file
if (ofGetElapsedTimeMillis() - logTimer >= 60000) {
// Get the temperature
FILE *temperatureFile;
double T;
temperatureFile = fopen ("/sys/class/thermal/thermal_zone0/temp", "r");
if (temperatureFile == NULL) {
cout << "Failed to read temp file\n";
} else {
fscanf (temperatureFile, "%lf", &T);
T /= 1000;
//printf ("The temperature is %6.3f C.\n", T);
fclose (temperatureFile);
}
// Log the data
logTimer = ofGetElapsedTimeMillis();
//string logFileName = "/logs/livestream/livestream01.log";
string logFileName = "/logs/livestream/livestream_" + hostname + ".log";
ofstream mFile;
mFile.open(logFileName.c_str(), ios::out | ios::app);
mFile << getDateTimeString() << ",TC," << T;
for (int j=0; j<nSensors; j++) {
float tempData = tempSensor.read(j);
mFile << ",T" << j << "," << tempData;
}
mFile << ",DL," << smoothPwm << " cm" << ",LR," << ofGetFrameRate() << "Hz,";
mFile << endl;
mFile.close();
}
}
