Skip to content

Commit

Permalink
/components/cyberdyne added pause & motion reverse to the component
Browse files Browse the repository at this point in the history
  • Loading branch information
@grotius-cnc
grotius-cnc committed Nov 14, 2023
1 parent 520cdff commit 5f32673
Show file tree
Hide file tree
Showing 4 changed files with 89 additions and 26 deletions.
88 changes: 65 additions & 23 deletions cmake/components/cyberdyne/cyberdyne.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,8 @@

/* module information */
MODULE_AUTHOR("Skynet");
MODULE_DESCRIPTION("Halmodule test");
MODULE_DESCRIPTION("Halmodule cyberdyne"
"Testing the ruckig-dev scurve library to use velocity end values.");
MODULE_LICENSE("GPL");

static int comp_idx;
Expand All @@ -27,12 +28,13 @@ typedef struct {
} float_data_t;
float_data_t *vel, *pos, *acc, *return_code;
float_data_t *maxjerk, *maxacc, *tarpos, *tarvel, *cycletime, *tarvel, *maxvel;
float_data_t *waypoint;

//! Pins
typedef struct {
hal_bit_t *Pin;
} bit_data_t;
bit_data_t *module;
bit_data_t *module, *pause_, *reverse;

typedef struct {
hal_s32_t *Pin;
Expand Down Expand Up @@ -101,7 +103,7 @@ int rtapi_app_main(void) {

void set_ruckig_values(){

//! Set some values.
//! Set some values if using in manual mode, set mode_auto=0;
*maxvel->Pin=50;
*maxjerk->Pin=1100;
*maxacc->Pin=500;
Expand All @@ -112,16 +114,24 @@ void set_ruckig_values(){

void set_ruckig_waypoints(){

//! A little trajectory using a few waypoints, using velocity end values.
struct ruckig_c_waypoint waypoint;
waypoint.goalpos=100;
waypoint.ve=10;
ruckig_add_waypoint(ruckig_ptr,waypoint);

waypoint.starpos=0;
waypoint.goalpos=200;
waypoint.vo=0;
waypoint.ve=25;
ruckig_add_waypoint(ruckig_ptr,waypoint);

waypoint.starpos=200;
waypoint.goalpos=500;
waypoint.vo=25;
waypoint.ve=10;
ruckig_add_waypoint(ruckig_ptr,waypoint);

waypoint.starpos=500;
waypoint.goalpos=1000;
waypoint.vo=10;
waypoint.ve=0;
ruckig_add_waypoint(ruckig_ptr,waypoint);

Expand All @@ -134,58 +144,87 @@ void set_ruckig_waypoints(){
}

void rtapi_app_exit(void){
//! Free memory of the ruckig pointer.
ruckig_ptr=NULL;
hal_exit(comp_idx);
}

//! Perforn's every ms.
static void the_function(){

if(*module->Pin==1){}

*vel->Pin=r.newvel;
*acc->Pin=r.newacc;
*pos->Pin=r.newpos;
*vel->Pin=r.newvel; //! Update actual tp value to show in halscope.
*acc->Pin=r.newacc; //!
*pos->Pin=r.newpos; //!

//! Everything is ok.
*return_code->Pin=r.function_return_code;

r.enable=1;
r.control_interfacetype=position;
r.durationdiscretizationtype=Continuous;
r.synchronizationtype=None;
r.enable=1; //! Enable ruckig.
r.control_interfacetype=position; //! For normal usage, for pause we use type velocity.
r.durationdiscretizationtype=Continuous; //! Every trajectory duration is allowed (Default)
r.synchronizationtype=None; //! Calculate every DoF independently

r.cycletime=*cycletime->Pin; // 0.001
r.maxacc=*maxacc->Pin; // 500
r.maxjerk=*maxjerk->Pin; // 1100
r.maxvel=*maxvel->Pin; // 50
r.pause=*pause_->Pin; //! Pause pin.
r.reverse=*reverse->Pin; //! Motion reverse pin.

//! If used for manual hal pin inputs.
if(!mode_auto){
r.tarpos=*tarpos->Pin;
r.tarvel=*tarvel->Pin;
} else {
r.tarpos=ruckig_get_waypoint(ruckig_ptr,waypoint_nr).goalpos;
r.tarvel=ruckig_get_waypoint(ruckig_ptr,waypoint_nr).ve;
//! Use in auto mode, using waypoints.
if(!r.reverse){ //! Motion forward, goto goalpos of segment.
r.tarpos=ruckig_get_waypoint(ruckig_ptr,waypoint_nr).goalpos;
r.tarvel=ruckig_get_waypoint(ruckig_ptr,waypoint_nr).ve;
} else { //! Motion reverse, goto begin of segment.
r.tarpos=ruckig_get_waypoint(ruckig_ptr,waypoint_nr).starpos;
r.tarvel=ruckig_get_waypoint(ruckig_ptr,waypoint_nr).vo;
}
}

if(ruckig_calculate_c(r,&r)){}
//! Perform the ruckig scurve traject calculation.
ruckig_calculate_c(r,&r);

//! Returncode 1=finished.
if(r.function_return_code==1){

//! Size of the waypoint bucket.
int size=ruckig_waypoint_vector_size(ruckig_ptr);
if(waypoint_nr<size-1){
waypoint_nr++;
printf("waypoint nr: %i \n",waypoint_nr);

//! Motion forward, increment to next segment.
if(!r.reverse){
if(waypoint_nr<size-1){
waypoint_nr++;
// printf("waypoint forward nr: %i \n",waypoint_nr);
}
} else { //! Motion reverse, decrement on segment.
if(waypoint_nr>0){
waypoint_nr--;
// printf("waypoint reverse nr: %i \n",waypoint_nr);
}
}
}
//! Show the outside world wich waypoint we are.
*waypoint->Pin=waypoint_nr;
}

//! Setup hal pins to interact outside the golden ring.
static int setup_pins(){
int r=0;

module = (bit_data_t*)hal_malloc(sizeof(bit_data_t));
r+=hal_pin_bit_new("cyberdyne.enable",HAL_IN,&(module->Pin),comp_idx);

pause_ = (bit_data_t*)hal_malloc(sizeof(bit_data_t));
r+=hal_pin_bit_new("cyberdyne.pause",HAL_IN,&(pause_->Pin),comp_idx);

reverse = (bit_data_t*)hal_malloc(sizeof(bit_data_t));
r+=hal_pin_bit_new("cyberdyne.reverse",HAL_IN,&(reverse->Pin),comp_idx);

vel = (float_data_t*)hal_malloc(sizeof(float_data_t));
r+=hal_pin_float_new("cyberdyne.vel",HAL_OUT,&(vel->Pin),comp_idx);

Expand All @@ -199,7 +238,7 @@ static int setup_pins(){
r+=hal_pin_float_new("cyberdyne.code",HAL_OUT,&(return_code->Pin),comp_idx);

tarvel = (float_data_t*)hal_malloc(sizeof(float_data_t));
r+=hal_pin_float_new("cyberdyne.tarvel",HAL_IN,&(tarvel->Pin),comp_idx);
r+=hal_pin_float_new("cyberdyne.tarvel_manual_mode",HAL_IN,&(tarvel->Pin),comp_idx);

maxvel = (float_data_t*)hal_malloc(sizeof(float_data_t));
r+=hal_pin_float_new("cyberdyne.maxvel",HAL_IN,&(maxvel->Pin),comp_idx);
Expand All @@ -211,11 +250,14 @@ static int setup_pins(){
r+=hal_pin_float_new("cyberdyne.maxjerk",HAL_IN,&(maxjerk->Pin),comp_idx);

tarpos = (float_data_t*)hal_malloc(sizeof(float_data_t));
r+=hal_pin_float_new("cyberdyne.tarpos",HAL_IN,&(tarpos->Pin),comp_idx);
r+=hal_pin_float_new("cyberdyne.tarpos_manual_mode",HAL_IN,&(tarpos->Pin),comp_idx);

cycletime = (float_data_t*)hal_malloc(sizeof(float_data_t));
r+=hal_pin_float_new("cyberdyne.cycletime",HAL_IN,&(cycletime->Pin),comp_idx);

waypoint = (float_data_t*)hal_malloc(sizeof(float_data_t));
r+=hal_pin_float_new("cyberdyne.waypoint",HAL_OUT,&(waypoint->Pin),comp_idx);

return r;
}

Expand Down
9 changes: 7 additions & 2 deletions cmake/components/cyberdyne/ruckig_dev_format.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,10 +41,13 @@ struct ruckig_c_data {
double curvel, curacc, curpos;
double newvel, newacc, newpos;
double tarvel, taracc, tarpos;
double oldmaxvel, oldmaxacc, oldmaxjerk, oldtarvel, oldtaracc, oldtarpos;
double oldmaxvel, oldmaxacc, oldmaxjerk, oldtarvel, oldtaracc, oldtarpos; //! Used to trigger interupts.
bool oldpause, oldreverse; //! Used to trigger interupts.
double maxvel,maxacc,maxjerk;
bool enable; //! Enable ruckig.
bool initialized;
bool initialized; //! New motion has to be initialized.
bool pause; //! Pause the motion.
bool reverse; //! Motion reverse direction.
enum ruckig_c_control_interface control_interfacetype;
enum ruckig_c_synchronization synchronizationtype;
enum ruckig_c_durationdiscretization durationdiscretizationtype;
Expand All @@ -55,7 +58,9 @@ struct ruckig_c_data {
};

struct ruckig_c_waypoint {
double vo; //! Velocity begin.
double ve; //! Velocity end.
double starpos; //! Position start.
double goalpos; //! Position target.
};

Expand Down
15 changes: 14 additions & 1 deletion cmake/components/cyberdyne/ruckig_dev_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,16 +25,20 @@ extern "C" int ruckig_calculate_c(struct ruckig_c_data in, ruckig_c_data *out){
return ruckig_dev_interface().ruckig_calculate(in,*out);
}

//! Holds the calculated trajectory data for one traject.
ruckig::Trajectory<1> trajectory;

//! Main ruckig trajectory function.
//! This function calculates the traject, and can handle interupts like pause etc.
int ruckig_dev_interface::ruckig_calculate(ruckig_c_data in, ruckig_c_data &out){

out=in;
out.at_time+=out.cycletime;

//! Check for all kind of interupts.
if(out.maxvel!=out.oldmaxvel || out.maxacc!=out.oldmaxacc || out.maxjerk!=out.oldmaxjerk ||
out.tarvel!=out.oldtarvel || out.taracc!=out.oldtaracc || out.tarpos!=out.oldtarpos ){
out.tarvel!=out.oldtarvel || out.taracc!=out.oldtaracc || out.tarpos!=out.oldtarpos ||
out.pause!=out.oldpause || out.reverse!=out.oldreverse ){
// printf("Ruckig interupt. \n");

//! For new calculation, set actual positions.
Expand All @@ -55,6 +59,13 @@ int ruckig_dev_interface::ruckig_calculate(ruckig_c_data in, ruckig_c_data &out)

// printf("Ruckig calculate new motion. \n");

if(out.pause){
input.control_interface=ruckig::ControlInterface::Velocity;
input.synchronization=ruckig::Synchronization::None;
input.target_velocity[0]=0;
input.target_acceleration[0]=0;
}

//! Calculate the trajectory in an offline manner (outside of the control loop)
//! This is done to avoid a velocity end error when using the online trajectory.
ruckig::Ruckig<1> otg;
Expand All @@ -72,6 +83,8 @@ int ruckig_dev_interface::ruckig_calculate(ruckig_c_data in, ruckig_c_data &out)
out.oldtarvel=out.tarvel;
out.oldtaracc=out.taracc;
out.oldtarpos=out.tarpos;
out.oldpause=out.pause;
out.oldreverse=out.reverse;

out.at_time=0;
out.initialized=1;
Expand Down
3 changes: 3 additions & 0 deletions cmake/components/cyberdyne/ruckig_dev_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,14 @@ class ruckig_dev_interface
public:
ruckig_dev_interface();

//! Data converter functions.
ruckig::InputParameter<1> ruckig_c_data_to_cpp(ruckig_c_data input);
ruckig_c_data ruckig_cpp_data_to_c(ruckig::InputParameter<1> input);

//! Main ruckig trajectory function.
int ruckig_calculate(ruckig_c_data in, ruckig_c_data &out);

//! A bucket that stores our waypoints.
std::vector<ruckig_c_waypoint> pointvec;
};

Expand Down

0 comments on commit 5f32673

Please sign in to comment.