/// 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();
}
