int main() { lidar_init(); static int k = 1; int i; for (i = 0; i < 100; i++) { lidar_fp = fopen("/root/lidarProfileTest", "w"); if (urg_start_measurement(&lidar, URG_DISTANCE, 1, 0) < 0) { printf("ERROR: unable to start measurement on LIDAR\n"); } length_data_size = urg_get_distance(&lidar, length_data, NULL); if (length_data_size <= 0) { printf("ERROR: unable to get distance data from LIDAR\n"); } fprintf(lidar_fp, "%i\t%0.2f", k++, 1.00); int i; for (i = 0; i < length_data_size; i++) { fprintf(lidar_fp, "\t%ld", length_data[i]); } fprintf(lidar_fp, "\n"); fclose(lidar_fp); } return 0; }
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; }
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::setup(){ ofSetVerticalSync(false); //ofSetFrameRate(10000) counter = 0; blinkTimer = ofGetSystemTimeMicros(); logTimer = ofGetElapsedTimeMillis(); // Setup GPIOs gpio15 = new GPIO("15"); gpio15->export_gpio(); gpio15->setdir_gpio("out"); gpio15->setval_gpio("0"); gpio15outState = false; gpio21 = new GPIO("21"); gpio21->export_gpio(); gpio21->setdir_gpio("out"); gpio21->setval_gpio("0"); gpio21outState = false; // PWM smoothing parameters nSamplesToSmooth = 0; maxSamplesToSmooth = 1; // determines smoothing smoothPwm = 0; // smoothed PWM value minDist = 30; //cm maxDist = 30*20; //cm // Sound output pitchSound.loadSound("sounds/INT18PIPES1.mp3"); pitchSound.setVolume(0.3f); pitchSound.setMultiPlay(false); volSound.loadSound("sounds/Tp10.wav"); volSound.setVolume(0.3f); volSound.setMultiPlay(true); ofSoundSetVolume(1.0f); volBend = true; pitchBend = false; // Init I2C fd = lidar_init(false); if (fd == -1) { printf("initialization error\n"); ofApp::exit(); } // Get the host name int z; char buf[32]; z = gethostname(buf,sizeof buf); if ( z != -1 ) { hostname = string(buf); //printf("host name = '%s'\n",buf); cout << "host name = " << hostname << endl; } else { cout << "host name unknown" << endl; hostname = "unknown"; } // Write initialization to the log string logFileName = "/logs/livestream/livestream_" + hostname + ".log"; ofstream mFile; mFile.open(logFileName.c_str(), ios::out | ios::app); mFile << getDateTimeString() << ",INITILIZATION" << endl; mFile.close(); nSensors = tempSensor.listDevices(); }