/// find Y MAX endstop void home_y_positive() { queue_wait(); #if defined Y_MAX_PIN uint8_t denoise_count = 0; // home Y y_enable(); // hit home hard y_direction(1); while (denoise_count < 8) { // denoise if (y_max()) denoise_count++; else denoise_count = 0; // step y_step(); delay(5); unstep(); // wait until neyt step time delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Y / ((float) MAXIMUM_FEEDRATE_Y))); } denoise_count = 0; // back off slowly y_direction(0); while (y_max() != 0) { // step y_step(); delay(5); unstep(); // wait until next step time delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Y / ((float) SEARCH_FEEDRATE_Y))); } // set Y home TARGET t = {0, 0, 0, 0, 0}; // set position to MAX startpoint.Y = current_position.Y = (int32_t) (Y_MAX * STEPS_PER_MM_Y); // go to zero t.F = MAXIMUM_FEEDRATE_Y; enqueue(&t); #endif }
/// fund Y MIN endstop void home_y_negative() { queue_wait(); #if defined Y_MIN_PIN uint8_t denoise_count = 0; // home Y y_enable(); // hit home hard y_direction(0); while (denoise_count < 8) { // denoise if (y_min()) denoise_count++; else denoise_count = 0; // step y_step(); delay(5); unstep(); // wait until neyt step time delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Y / ((float) MAXIMUM_FEEDRATE_Y))); } denoise_count = 0; // back off slowly y_direction(1); while (y_min() != 0) { // step y_step(); delay(5); unstep(); // wait until next step time delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Y / ((float) SEARCH_FEEDRATE_Y))); } // set Y home startpoint.Y = current_position.Y = 0; #endif }
void TransformCloud() { /// get the original point, X and Y direction normal of the new coordinate Eigen::Affine3f transform; Eigen::Vector3f y_direction, z_axis, origin; origin(0) = picked_points[0].x; origin(1) = picked_points[0].y; origin(2) = picked_points[0].z; y_direction(0) = picked_points[2].x - picked_points[1].x; y_direction(1) = picked_points[2].y - picked_points[1].y; y_direction(2) = picked_points[2].z - picked_points[1].z; z_axis(0) = build_plane_coeff(0); z_axis(1) = build_plane_coeff(1); z_axis(2) = build_plane_coeff(2); y_direction.normalize(); z_axis.normalize(); double result2 = y_direction(0) * z_axis(0) + y_direction(1) * z_axis(1) + y_direction(2) * z_axis(2); cout << "Test orthogonal: " << result2 << endl; // Get transformation matrix pcl::getTransformationFromTwoUnitVectorsAndOrigin(y_direction, z_axis, origin, transform); // transform the cloud pcl::transformPointCloud(*build_cloud_accurate_plane, *build_cloud_transformed_plane, transform); pcl::transformPointCloud(*build_cloud_raw, *build_cloud_transformed_raw, transform); // save... pcl::io::savePCDFileASCII(cmd_arguments.pt_pcd_, *build_cloud_transformed_plane); cout << cmd_arguments.pt_pcd_ << " is saved.\n"; pcl::io::savePCDFileASCII(cmd_arguments.rt_pcd_, *build_cloud_transformed_raw); cout << cmd_arguments.rt_pcd_ << " is saved.\n"; viewer->updatePointCloud(build_cloud_transformed_plane, "build plane cloud"); viewer->initCameraParameters(); viewer->removeAllShapes(); // get the bounder box of the transformed cloud Create_building_plane_bounder_box_and_add_to_viewer(); }