Skip to content

Commit

Permalink
added code to cut 2d circle.
Browse files Browse the repository at this point in the history
  • Loading branch information
@grotius-cnc
grotius-cnc committed Nov 10, 2023
1 parent 884ce9f commit 9f3936d
Show file tree
Hide file tree
Showing 6 changed files with 112 additions and 41 deletions.
4 changes: 3 additions & 1 deletion configs/sim/axis/halshow.preferences
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,15 @@
# Halshow settings
# This file is generated automatically.
wm geometry . 753x802+2654+212
wm geometry . 753x802+1696+596
placeFrames 0.42005813953488375
set ::ratio 0.42005813953488375
set ::old_w_leftf 160
set ::watchlist {
pin+tpmod_scurve_skynet.curacc
pin+tpmod_scurve_skynet.curvel
pin+tpmod_scurve_skynet.reverse
param+tpmod_scurve_skynet.look_ahead
param+tpmod_scurve_skynet.vel_end
}
set ::workmode watchhal
set ::watchInterval 100
Expand Down
13 changes: 13 additions & 0 deletions nc_files/circle_100.ngc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
( File created using Intuwiz Software Service )
( https://www.intuwiz.com )
( More information: https://www.intuwiz.com/circle.html )
( File created: 2023-11-10 09:20:39 UTC )

G00 Z10
G00 X0 Y0
G01 Z0 F1000
G02 I50
G00 Z10
G00 X0 Y0
M30

57 changes: 45 additions & 12 deletions src/emc/tp/tp_arcs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,24 @@ tp_arcs::tp_arcs()

}

void tp_arcs::sc_interpolate_arc(sc_pnt p0_,
sc_pnt p1_,
sc_pnt p2_,
void tp_arcs::sc_interpolate_arc(sc_pnt p0_, //! Startpoint.
sc_pnt p1_, //! Waypoint.
sc_pnt p2_, //! Endpoint.
sc_pnt p3_, //! Centerpoint.
double progress,
sc_pnt &pi){


//! Is it a circle?
if(p0_.x==p2_.x && p0_.y==p2_.y && p0_.z==p2_.z){
printf("interpolating arc as 2d circle. \n");

sc_pnt p33_=p3_;
p33_.z+=1;
pi=sc_rotate_point_around_line(p0_,2*M_PI*progress,p3_,p33_);
return;
}

Eigen::Vector3d p1(p0_.x,p0_.y,p0_.z);
Eigen::Vector3d p2(p1_.x,p1_.y,p1_.z);
Eigen::Vector3d p3(p2_.x,p2_.y,p2_.z);
Expand Down Expand Up @@ -92,9 +104,9 @@ void tp_arcs::sc_interpolate_arc(sc_pnt p0_,
pi=sc_rotate_point_around_line({p1.x(),p1.y(),p1.z()},progress*angle,{pc.x(),pc.y(),pc.z()},{pc.x()+an.x(),pc.y()+an.y(),pc.z()+an.z()});
}

extern "C" void interpolate_arc_c(struct sc_pnt p0, struct sc_pnt p1, struct sc_pnt p2, double progress, struct sc_pnt *pi){
extern "C" void interpolate_arc_c(struct sc_pnt p0, struct sc_pnt p1, struct sc_pnt p2, struct sc_pnt p3, double progress, struct sc_pnt *pi){
sc_pnt p;
tp_arcs().sc_interpolate_arc(p0,p1,p2,progress,p);
tp_arcs().sc_interpolate_arc(p0,p1,p2,p3,progress,p);
*pi=p;
}

Expand Down Expand Up @@ -137,9 +149,20 @@ void tp_arcs::sc_arc_radius(sc_pnt p0,
radius = (pcenter-pa).norm();
}

double tp_arcs::sc_arc_lenght(sc_pnt p0,
sc_pnt p1,
sc_pnt p2){
double tp_arcs::sc_arc_lenght(sc_pnt p0, //! Start
sc_pnt p1, //! Waypoint
sc_pnt p2, //! Endpoint
sc_pnt p3){ //! Center

//! Is it a circle?
if(p0.x==p2.x && p0.y==p2.y && p0.z==p2.z){
printf("processing arc lenght for circle. \n");
double radius=sqrt(pow(p3.x-p0.x,2)+pow(p3.y-p0.y,2)+pow(p3.z-p0.z,2));
printf("radius: %f \n",radius);

double circumfece=(M_PI*2)*radius;
return circumfece;
}

Eigen::Vector3d pa,pb,pc;
pa.x()=p0.x;
Expand Down Expand Up @@ -170,11 +193,11 @@ double tp_arcs::sc_arc_lenght(sc_pnt p0,
//! Center of arc.
Eigen::Vector3d pcenter = pa + v1*k1 + v2*k2;
arc.center={pcenter.x(),pcenter.y(),pcenter.z()};
//! std::cout<<"arc center x:"<<pc.x()<<" y:"<<pc.y()<<" z:"<<pc.z()<<std::endl;
std::cout<<"arc center x:"<<pc.x()<<" y:"<<pc.y()<<" z:"<<pc.z()<<std::endl;

double radius = (pcenter-pa).norm();
arc.radius=radius;
//! std::cout<<"radius: "<<radius<<std::endl;
std::cout<<"radius: "<<radius<<std::endl;
arc.diameter=radius*2;

//! Arc angle.
Expand Down Expand Up @@ -228,8 +251,8 @@ double tp_arcs::sc_arc_lenght(sc_pnt p0,
return arc.arcLenght;
}

extern "C" double arc_lenght_c(struct sc_pnt start, struct sc_pnt way, struct sc_pnt end){
double l=tp_arcs().sc_arc_lenght(start,way,end);
extern "C" double arc_lenght_c(struct sc_pnt start, struct sc_pnt way, struct sc_pnt end, struct sc_pnt center){
double l=tp_arcs().sc_arc_lenght(start,way,end,center);
if(isnanf(l)){
return 0;
}
Expand Down Expand Up @@ -382,6 +405,16 @@ void tp_arcs::sc_arc_get_mid_waypoint(sc_pnt p0, //! Start.
sc_pnt p2, //! End.
sc_pnt &pi){

//! Is it a circle?
if(p0.x==p2.x && p0.y==p2.y && p0.z==p2.z){
printf("no arc but circle, processed as 2d circle. \n");

sc_pnt p11=p1;
p11.z+=100;
pi=sc_rotate_point_around_line(p0,M_PI,p1,p11);
return;
}

sc_arc arc;
arc.center={p1.x ,p1.y ,p1.z};

Expand Down
31 changes: 17 additions & 14 deletions src/emc/tp/tp_arcs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,30 +21,33 @@ class tp_arcs
public:
tp_arcs();

void sc_interpolate_arc(sc_pnt p0,
sc_pnt p1, //! Waypoint.
sc_pnt p2,
void sc_interpolate_arc(sc_pnt p0, //! Startpoint.
sc_pnt p1, //! Waypoint.
sc_pnt p2, //! Endpoint.
sc_pnt p3_, //! Centerpoint.
double progress, //! 0-1.
sc_pnt &pi); //! Interpolated point.
sc_pnt &pi); //! Interpolated point.

double sc_arc_lenght(sc_pnt p0,
sc_pnt p1, //! Waypoint.
sc_pnt p2);
double sc_arc_lenght(sc_pnt p0, //! Start.
sc_pnt p1, //! Waypoint.
sc_pnt p2, //! End.
sc_pnt p3); //! Center.

void sc_arc_radius(sc_pnt p0,
sc_pnt p1, //! Waypoint.
sc_pnt p2,
double &radius);
sc_pnt p1, //! Waypoint.
sc_pnt p2,
double &radius);

//! If points are colinear, output is xy plane, type clockwise g2.
void sc_arc_get_mid_waypoint(sc_pnt p0, //! Start.
sc_pnt p1, //! Center.
sc_pnt p2, //! End.
sc_pnt &pi);
sc_pnt p1, //! Center.
sc_pnt p2, //! End.
sc_pnt &pi);

private:
sc_pnt sc_rotate_point_around_line(sc_pnt thePointToRotate,
double theta,sc_pnt theLineP1,
double theta, //! Input in radians.
sc_pnt theLineP1,
sc_pnt theLineP2);

//! Keeping as is from original github code example.
Expand Down
14 changes: 7 additions & 7 deletions src/emc/tp/tp_corners.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@ void tp_corners::line_arc_angle(sc_pnt p0,
sc_pnt p3,
double &angle_deg){

sc_pnt pi;
tp_arcs().sc_interpolate_arc(p1,p2,p3,0.1,pi);
sc_pnt pi,pdummy;
tp_arcs().sc_interpolate_arc(p1,p2,p3,pdummy,0.1,pi);
line_line_angle(p0,p1,pi,angle_deg);
}

Expand All @@ -38,8 +38,8 @@ void tp_corners::arc_line_angle(sc_pnt p0,
sc_pnt p3,
double &angle_deg){

sc_pnt pi;
tp_arcs().sc_interpolate_arc(p0,p1,p2,0.9,pi);
sc_pnt pi,pdummy;
tp_arcs().sc_interpolate_arc(p0,p1,p2,pdummy,0.9,pi);
line_line_angle(pi,p2,p3,angle_deg);
}

Expand All @@ -50,9 +50,9 @@ void tp_corners::arc_arc_angle(sc_pnt p0,
sc_pnt p4,
double &angle_deg){

sc_pnt pi0, pi1;
tp_arcs().sc_interpolate_arc(p0,p1,p2,0.9,pi0);
tp_arcs().sc_interpolate_arc(p2,p3,p4,0.1,pi1);
sc_pnt pi0, pi1, pdummy;
tp_arcs().sc_interpolate_arc(p0,p1,p2,pdummy,0.9,pi0);
tp_arcs().sc_interpolate_arc(p2,p3,p4,pdummy,0.1,pi1);
line_line_angle(pi0,p2,pi1,angle_deg);
}

Expand Down
34 changes: 27 additions & 7 deletions src/emc/tp/tp_scurve_skynet.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ typedef struct {
typedef struct {
hal_float_t Pin;
} param_float_data_t;
param_float_data_t *tp_progress;
param_float_data_t *test_param;

typedef struct {
hal_bit_t Pin;
Expand Down Expand Up @@ -134,8 +134,8 @@ static int setup_pins(){
max_look_ahead = (param_s32_data_t*)hal_malloc(sizeof(param_s32_data_t));
r+=hal_param_s32_new("tpmod_scurve_skynet.look_ahead",HAL_RW,&(max_look_ahead->Pin),comp_idx);

tp_progress = (param_float_data_t*)hal_malloc(sizeof(param_float_data_t));
r+=hal_param_float_new("tpmod_scurve_skynet.progress",HAL_RW,&(tp_progress->Pin),comp_idx);
test_param = (param_float_data_t*)hal_malloc(sizeof(param_float_data_t));
r+=hal_param_float_new("tpmod_scurve_skynet.vel_end",HAL_RW,&(test_param->Pin),comp_idx);

clear_vec = (param_bit_data_t*)hal_malloc(sizeof(param_bit_data_t));
r+=hal_param_bit_new("tpmod_scurve_skynet.clear_vec",HAL_RW,&(clear_vec->Pin),comp_idx);
Expand Down Expand Up @@ -238,12 +238,12 @@ extern void vector_add_segment(struct tp_vector *ptr, struct tp_segment b);
extern void vector_remove_last_segment(struct tp_vector *ptr);
extern void vector_set_end_angle(tp_vector *ptr, int index, double angle_deg);

extern double arc_lenght_c(struct sc_pnt start, struct sc_pnt way, struct sc_pnt end);
extern double arc_lenght_c(struct sc_pnt start, struct sc_pnt way, struct sc_pnt end, struct sc_pnt center);
extern double line_lenght_c(struct sc_pnt start, struct sc_pnt end);
extern void interpolate_line_c(struct sc_pnt p0, struct sc_pnt p1, double progress, struct sc_pnt *pi);
extern void interpolate_dir_c(struct sc_dir p0, struct sc_dir p1, double progress, struct sc_dir *pi);
extern void interpolate_ext_c(struct sc_ext p0, struct sc_ext p1, double progress, struct sc_ext *pi);
extern void interpolate_arc_c(struct sc_pnt p0, struct sc_pnt p1, struct sc_pnt p2, double progress, struct sc_pnt *pi);
extern void interpolate_arc_c(struct sc_pnt p0, struct sc_pnt p1, struct sc_pnt p2, struct sc_pnt p3, double progress, struct sc_pnt *pi);
extern void sc_arc_get_mid_waypoint_c(struct sc_pnt start, struct sc_pnt center, struct sc_pnt end, struct sc_pnt *waypoint);
extern void vector_interpolate_traject_c(struct tp_vector *ptr, double traject_progress, double traject_lenght, double *curve_progress, int *curve_nr);

Expand Down Expand Up @@ -319,8 +319,27 @@ int tpCreate(TP_STRUCT * const tp, int _queueSize,int id)
printf("tpCreate, set look_ahead to : %i \n",max_look_ahead->Pin);
}

test_param->Pin=0;

printf("tpCreate. set tp->queuesize to: %i \n", tp->queueSize);


//! Test if a circle is processed ok.
struct sc_pnt start={0,0,0};
struct sc_pnt center={50,0,0};
struct sc_pnt end={0,0,0};
struct sc_pnt way={0,0,0};

//! Create a 3d arc using waypoint technique.
sc_arc_get_mid_waypoint_c(start,
center,
end,
&way);

printf("Testing circle, start{0,0,0} center{50,0,0} end {0,0,0} \n");
printf("waypoint rotated at Pi radians x : %f y: %f z: %f \n",way.x,way.y,way.z);
printf("arc lenght: %f \n", arc_lenght_c(start,way,end,center));

return 0;
}

Expand Down Expand Up @@ -749,7 +768,7 @@ int tpAddCircle(TP_STRUCT * const tp,
b.vm=vel;
b.ve=0;

b.path_lenght=arc_lenght_c(b.pnt_s,b.pnt_w,b.pnt_e);
b.path_lenght=arc_lenght_c(b.pnt_s,b.pnt_w,b.pnt_e,b.pnt_c);

//! Calculate the arc radius, we can use this for look ahead of tiny arc's.
b.radius=arc_radius(b.pnt_w,b.pnt_c);
Expand Down Expand Up @@ -833,6 +852,7 @@ inline void update_gui(TP_STRUCT * const tp){
interpolate_arc_c(vector_at(vector_ptr,id).pnt_s,
vector_at(vector_ptr,id).pnt_w,
vector_at(vector_ptr,id).pnt_e,
vector_at(vector_ptr,id).pnt_c,
tp->segment_progress,
&xyz);
}
Expand Down Expand Up @@ -926,7 +946,7 @@ inline void update_ruckig(TP_STRUCT * const tp){
}

r.enable=1;
r.durationdiscretizationtype=Continuous;
r.durationdiscretizationtype=Discrete;
r.synchronizationtype=None;

//! MENTION: tp->cycletime is not set to 0.001, or it has a long to double conversion error.
Expand Down

0 comments on commit 9f3936d

Please sign in to comment.