Skip to content

Commit 565bde3

Browse files
BsAtHomeBsAtHome
committed
hal: Update motion to getter/setter.
1 parent d5c3687 commit 565bde3

6 files changed

Lines changed: 574 additions & 629 deletions

File tree

‎src/emc/motion/axis.c‎

Lines changed: 69 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -28,26 +28,26 @@ typedef struct {
2828
} emcmot_axis_t;
2929

3030
typedef struct {
31-
hal_float_t *pos_cmd; /* RPI: commanded position */
32-
hal_float_t *teleop_vel_cmd; /* RPI: commanded velocity */
33-
hal_float_t *teleop_pos_cmd; /* RPI: teleop traj planner pos cmd */
34-
hal_float_t *teleop_vel_lim; /* RPI: teleop traj planner vel limit */
35-
hal_bit_t *teleop_tp_enable; /* RPI: teleop traj planner is running */
36-
37-
hal_s32_t *ajog_counts; /* WPI: jogwheel position input */
38-
hal_bit_t *ajog_enable; /* RPI: enable jogwheel */
39-
hal_float_t *ajog_scale; /* RPI: distance to jog on each count */
40-
hal_float_t *ajog_accel_fraction; /* RPI: to limit wheel jog accel */
41-
hal_bit_t *ajog_vel_mode; /* RPI: true for "velocity mode" jogwheel */
42-
hal_bit_t *kb_ajog_active; /* RPI: executing keyboard jog */
43-
hal_bit_t *wheel_ajog_active; /* RPI: executing handwheel jog */
44-
45-
hal_bit_t *eoffset_enable;
46-
hal_bit_t *eoffset_clear;
47-
hal_s32_t *eoffset_counts;
48-
hal_float_t *eoffset_scale;
49-
hal_float_t *external_offset;
50-
hal_float_t *external_offset_requested;
31+
hal_real_t pos_cmd; /* RPI: commanded position */
32+
hal_real_t teleop_vel_cmd; /* RPI: commanded velocity */
33+
hal_real_t teleop_pos_cmd; /* RPI: teleop traj planner pos cmd */
34+
hal_real_t teleop_vel_lim; /* RPI: teleop traj planner vel limit */
35+
hal_bool_t teleop_tp_enable; /* RPI: teleop traj planner is running */
36+
37+
hal_sint_t ajog_counts; /* WPI: jogwheel position input */
38+
hal_bool_t ajog_enable; /* RPI: enable jogwheel */
39+
hal_real_t ajog_scale; /* RPI: distance to jog on each count */
40+
hal_real_t ajog_accel_fraction; /* RPI: to limit wheel jog accel */
41+
hal_bool_t ajog_vel_mode; /* RPI: true for "velocity mode" jogwheel */
42+
hal_bool_t kb_ajog_active; /* RPI: executing keyboard jog */
43+
hal_bool_t wheel_ajog_active; /* RPI: executing handwheel jog */
44+
45+
hal_bool_t eoffset_enable;
46+
hal_bool_t eoffset_clear;
47+
hal_sint_t eoffset_counts;
48+
hal_real_t eoffset_scale;
49+
hal_real_t external_offset;
50+
hal_real_t external_offset_requested;
5151
} axis_hal_t;
5252

5353

@@ -79,8 +79,8 @@ void axis_initialize_external_offsets(void)
7979
for (n = 0; n < EMCMOT_MAX_AXIS; n++) {
8080
axis_data = &hal_data->axis[n];
8181

82-
*(axis_data->external_offset) = 0;
83-
*(axis_data->external_offset_requested) = 0;
82+
hal_set_real(axis_data->external_offset, 0);
83+
hal_set_real(axis_data->external_offset_requested, 0);
8484
axis_array[n].ext_offset_tp.pos_cmd = 0;
8585
axis_array[n].ext_offset_tp.curr_pos = 0;
8686
axis_array[n].ext_offset_tp.curr_vel = 0;
@@ -100,15 +100,15 @@ static int export_axis(int mot_comp_id, char c, axis_hal_t * addr)
100100
msg = rtapi_get_msg_level();
101101
rtapi_set_msg_level(RTAPI_MSG_WARN);
102102

103-
CALL_CHECK(hal_pin_bit_newf(HAL_IN, &(addr->ajog_enable), mot_comp_id,"axis.%c.jog-enable", c));
104-
CALL_CHECK(hal_pin_float_newf(HAL_IN, &(addr->ajog_scale), mot_comp_id,"axis.%c.jog-scale", c));
105-
CALL_CHECK(hal_pin_s32_newf(HAL_IN, &(addr->ajog_counts), mot_comp_id,"axis.%c.jog-counts", c));
106-
CALL_CHECK(hal_pin_bit_newf(HAL_IN, &(addr->ajog_vel_mode), mot_comp_id,"axis.%c.jog-vel-mode", c));
107-
CALL_CHECK(hal_pin_bit_newf(HAL_OUT, &(addr->kb_ajog_active), mot_comp_id,"axis.%c.kb-jog-active", c));
108-
CALL_CHECK(hal_pin_bit_newf(HAL_OUT, &(addr->wheel_ajog_active), mot_comp_id,"axis.%c.wheel-jog-active", c));
103+
CALL_CHECK(hal_pin_new_bool(mot_comp_id, HAL_IN, &(addr->ajog_enable), 0, "axis.%c.jog-enable", c));
104+
CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_IN, &(addr->ajog_scale), 0.0, "axis.%c.jog-scale", c));
105+
CALL_CHECK(hal_pin_new_si32(mot_comp_id, HAL_IN, &(addr->ajog_counts), 0, "axis.%c.jog-counts", c));
106+
CALL_CHECK(hal_pin_new_bool(mot_comp_id, HAL_IN, &(addr->ajog_vel_mode), 0, "axis.%c.jog-vel-mode", c));
107+
CALL_CHECK(hal_pin_new_bool(mot_comp_id, HAL_OUT, &(addr->kb_ajog_active), 0, "axis.%c.kb-jog-active", c));
108+
CALL_CHECK(hal_pin_new_bool(mot_comp_id, HAL_OUT, &(addr->wheel_ajog_active), 0, "axis.%c.wheel-jog-active", c));
109109

110-
CALL_CHECK(hal_pin_float_newf(HAL_IN,&(addr->ajog_accel_fraction), mot_comp_id,"axis.%c.jog-accel-fraction", c));
111-
*addr->ajog_accel_fraction = 1.0; // fraction of accel for wheel ajogs
110+
// init: 1.0 fraction of accel for wheel ajogs
111+
CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_IN, &(addr->ajog_accel_fraction), 1.0, "axis.%c.jog-accel-fraction", c));
112112

113113
rtapi_set_msg_level(msg);
114114
return 0;
@@ -128,18 +128,18 @@ int axis_init_hal_io(int mot_comp_id)
128128
for (n = 0; n < EMCMOT_MAX_AXIS; n++) {
129129
char c = "xyzabcuvw"[n];
130130
axis_hal_t *axis_data = &(hal_data->axis[n]);
131-
CALL_CHECK(hal_pin_float_newf(HAL_OUT, &axis_data->pos_cmd, mot_comp_id, "axis.%c.pos-cmd", c));
132-
CALL_CHECK(hal_pin_float_newf(HAL_OUT, &axis_data->teleop_vel_cmd, mot_comp_id, "axis.%c.teleop-vel-cmd", c));
133-
CALL_CHECK(hal_pin_float_newf(HAL_OUT, &axis_data->teleop_pos_cmd, mot_comp_id, "axis.%c.teleop-pos-cmd", c));
134-
CALL_CHECK(hal_pin_float_newf(HAL_OUT, &axis_data->teleop_vel_lim, mot_comp_id, "axis.%c.teleop-vel-lim", c));
135-
CALL_CHECK(hal_pin_bit_newf(HAL_OUT, &axis_data->teleop_tp_enable, mot_comp_id, "axis.%c.teleop-tp-enable",c));
136-
CALL_CHECK(hal_pin_bit_newf(HAL_IN, &axis_data->eoffset_enable, mot_comp_id, "axis.%c.eoffset-enable", c));
137-
CALL_CHECK(hal_pin_bit_newf(HAL_IN, &axis_data->eoffset_clear, mot_comp_id, "axis.%c.eoffset-clear", c));
138-
CALL_CHECK(hal_pin_s32_newf(HAL_IN, &axis_data->eoffset_counts, mot_comp_id, "axis.%c.eoffset-counts", c));
139-
CALL_CHECK(hal_pin_float_newf(HAL_IN, &axis_data->eoffset_scale, mot_comp_id, "axis.%c.eoffset-scale", c));
140-
CALL_CHECK(hal_pin_float_newf(HAL_OUT, &axis_data->external_offset, mot_comp_id, "axis.%c.eoffset", c));
141-
CALL_CHECK(hal_pin_float_newf(HAL_OUT, &axis_data->external_offset_requested,
142-
mot_comp_id, "axis.%c.eoffset-request", c));
131+
CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &axis_data->pos_cmd, 0.0, "axis.%c.pos-cmd", c));
132+
CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &axis_data->teleop_vel_cmd, 0.0, "axis.%c.teleop-vel-cmd", c));
133+
CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &axis_data->teleop_pos_cmd, 0.0, "axis.%c.teleop-pos-cmd", c));
134+
CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &axis_data->teleop_vel_lim, 0.0, "axis.%c.teleop-vel-lim", c));
135+
CALL_CHECK(hal_pin_new_bool(mot_comp_id, HAL_OUT, &axis_data->teleop_tp_enable, 0, "axis.%c.teleop-tp-enable",c));
136+
CALL_CHECK(hal_pin_new_bool(mot_comp_id, HAL_IN, &axis_data->eoffset_enable, 0, "axis.%c.eoffset-enable", c));
137+
CALL_CHECK(hal_pin_new_bool(mot_comp_id, HAL_IN, &axis_data->eoffset_clear, 0, "axis.%c.eoffset-clear", c));
138+
CALL_CHECK(hal_pin_new_si32(mot_comp_id, HAL_IN, &axis_data->eoffset_counts, 0, "axis.%c.eoffset-counts", c));
139+
CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_IN, &axis_data->eoffset_scale, 0.0, "axis.%c.eoffset-scale", c));
140+
CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &axis_data->external_offset, 0.0, "axis.%c.eoffset", c));
141+
CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &axis_data->external_offset_requested,
142+
0.0, "axis.%c.eoffset-request", c));
143143

144144
retval = export_axis(mot_comp_id, c, axis_data);
145145
if (retval) {
@@ -159,16 +159,16 @@ void axis_output_to_hal(double *pcmd_p[])
159159
for (n = 0; n < EMCMOT_MAX_AXIS; n++) {
160160
emcmot_axis_t *axis = &axis_array[n];
161161
axis_hal_t *axis_data = &hal_data->axis[n];
162-
*(axis_data->teleop_vel_cmd) = axis->teleop_vel_cmd;
163-
*(axis_data->teleop_pos_cmd) = axis->teleop_tp.pos_cmd;
164-
*(axis_data->teleop_vel_lim) = axis->teleop_tp.max_vel;
165-
*(axis_data->teleop_tp_enable) = axis->teleop_tp.enable;
166-
*(axis_data->kb_ajog_active) = axis->kb_ajog_active;
167-
*(axis_data->wheel_ajog_active) = axis->wheel_ajog_active;
162+
hal_set_real(axis_data->teleop_vel_cmd, axis->teleop_vel_cmd);
163+
hal_set_real(axis_data->teleop_pos_cmd, axis->teleop_tp.pos_cmd);
164+
hal_set_real(axis_data->teleop_vel_lim, axis->teleop_tp.max_vel);
165+
hal_set_bool(axis_data->teleop_tp_enable, axis->teleop_tp.enable);
166+
hal_set_bool(axis_data->kb_ajog_active, axis->kb_ajog_active);
167+
hal_set_bool(axis_data->wheel_ajog_active, axis->wheel_ajog_active);
168168

169169
// hal pins: axis.L.pos-cmd reported without applied offsets:
170-
*(axis_data->pos_cmd) = *pcmd_p[n]
171-
- axis->ext_offset_tp.curr_pos;
170+
hal_set_real(axis_data->pos_cmd, *pcmd_p[n]
171+
- axis->ext_offset_tp.curr_pos);
172172
}
173173
}
174174

@@ -380,14 +380,14 @@ void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, boo
380380
axis_data = &hal_data->axis[axis_num];
381381

382382
// disallow accel bogus fractions
383-
if ( (*(axis_data->ajog_accel_fraction) > 1)
384-
|| (*(axis_data->ajog_accel_fraction) < 0) ) {
383+
if ( (hal_get_real(axis_data->ajog_accel_fraction) > 1)
384+
|| (hal_get_real(axis_data->ajog_accel_fraction) < 0) ) {
385385
aaccel_limit = axis->acc_limit;
386386
} else {
387-
aaccel_limit = *(axis_data->ajog_accel_fraction) * axis->acc_limit;
387+
aaccel_limit = hal_get_real(axis_data->ajog_accel_fraction) * axis->acc_limit;
388388
}
389389

390-
new_ajog_counts = *(axis_data->ajog_counts);
390+
new_ajog_counts = hal_get_si32(axis_data->ajog_counts);
391391
delta = new_ajog_counts - axis->old_ajog_counts;
392392
axis->old_ajog_counts = new_ajog_counts;
393393
if ( first_pass ) { continue; }
@@ -400,7 +400,7 @@ void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, boo
400400
return;
401401
}
402402
if (!motion_enable_flag) { continue; }
403-
if ( *(axis_data->ajog_enable) == 0 ) { continue; }
403+
if (!hal_get_bool(axis_data->ajog_enable)) { continue; }
404404
if (homing_is_active) { continue; }
405405
if (axis->kb_ajog_active) { continue; }
406406

@@ -411,9 +411,9 @@ void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, boo
411411
continue;
412412
}
413413

414-
distance = delta * *(axis_data->ajog_scale);
414+
distance = delta * hal_get_real(axis_data->ajog_scale);
415415
pos = axis->teleop_tp.pos_cmd + distance;
416-
if ( *(axis_data->ajog_vel_mode) ) {
416+
if ( hal_get_bool(axis_data->ajog_vel_mode) ) {
417417
double v = axis->vel_limit;
418418
/* compute stopping distance at max speed */
419419
stop_dist = v * v / ( 2 * aaccel_limit);
@@ -488,9 +488,7 @@ bool axis_plan_external_offsets(double servo_period, bool motion_enable_flag, bo
488488
int new_eoffset_counts, delta;
489489
static int last_eoffset_enable[EMCMOT_MAX_AXIS];
490490
double ext_offset_epsilon;
491-
hal_bit_t eoffset_active;
492-
493-
eoffset_active = 0;
491+
bool eoffset_active = 0;
494492

495493
for (n = 0; n < EMCMOT_MAX_AXIS; n++) {
496494
axis = &axis_array[n];
@@ -500,29 +498,29 @@ bool axis_plan_external_offsets(double servo_period, bool motion_enable_flag, bo
500498

501499
axis_data = &hal_data->axis[n];
502500

503-
new_eoffset_counts = *(axis_data->eoffset_counts);
501+
new_eoffset_counts = hal_get_si32(axis_data->eoffset_counts);
504502
delta = new_eoffset_counts - axis->old_eoffset_counts;
505503
axis->old_eoffset_counts = new_eoffset_counts;
506504

507-
*(axis_data->external_offset) = axis->ext_offset_tp.curr_pos;
505+
hal_set_real(axis_data->external_offset, axis->ext_offset_tp.curr_pos);
508506
axis->ext_offset_tp.enable = 1;
509507
if ( first_pass ) {
510-
*(axis_data->external_offset) = 0;
508+
hal_set_real(axis_data->external_offset, 0);
511509
continue;
512510
}
513511

514512
// Use stopping criterion of simple_tp.c:
515513
ext_offset_epsilon = TINY_DP(axis->ext_offset_tp.max_acc, servo_period);
516-
if (fabs(*(axis_data->external_offset)) > ext_offset_epsilon) {
514+
if (fabs(hal_get_real(axis_data->external_offset)) > ext_offset_epsilon) {
517515
eoffset_active = 1;
518516
}
519-
if ( !*(axis_data->eoffset_enable) ) {
517+
if ( !hal_get_bool(axis_data->eoffset_enable) ) {
520518
axis->ext_offset_tp.enable = 0;
521519
// Detect disabling of eoffsets:
522520
// At very high accel, simple planner may terminate with
523521
// a larger position value than occurs at more realistic accels.
524522
if (last_eoffset_enable[n]
525-
&& (fabs(*(axis_data->external_offset)) > ext_offset_epsilon)
523+
&& (fabs(hal_get_real(axis_data->external_offset)) > ext_offset_epsilon)
526524
&& motion_enable_flag
527525
&& axis->ext_offset_tp.enable) {
528526
// to stdout only:
@@ -531,7 +529,7 @@ bool axis_plan_external_offsets(double servo_period, bool motion_enable_flag, bo
531529
"*** External Offset disabled while NON-zero\n"
532530
"*** To clear: re-enable & zero or use Machine-Off\n",
533531
"XYZABCUVW"[n],
534-
*(axis_data->external_offset),
532+
hal_get_real(axis_data->external_offset),
535533
ext_offset_epsilon);
536534
}
537535
last_eoffset_enable[n] = 0;
@@ -540,17 +538,17 @@ bool axis_plan_external_offsets(double servo_period, bool motion_enable_flag, bo
540538
// useful for eoffset_pid hold
541539
}
542540
last_eoffset_enable[n] = 1;
543-
if (*(axis_data->eoffset_clear)) {
541+
if (hal_get_bool(axis_data->eoffset_clear)) {
544542
axis->ext_offset_tp.pos_cmd = 0;
545-
*(axis_data->external_offset_requested) = 0;
543+
hal_set_real(axis_data->external_offset_requested, 0);
546544
continue;
547545
}
548546
if (delta == 0) { continue; }
549547
if (!all_homed) { continue; }
550548
if (!motion_enable_flag) { continue; }
551549

552-
axis->ext_offset_tp.pos_cmd += delta * *(axis_data->eoffset_scale);
553-
*(axis_data->external_offset_requested) = axis->ext_offset_tp.pos_cmd;
550+
axis->ext_offset_tp.pos_cmd += delta * hal_get_real(axis_data->eoffset_scale);
551+
hal_set_real(axis_data->external_offset_requested, axis->ext_offset_tp.pos_cmd);
554552
} // for n
555553
first_pass = 0;
556554

0 commit comments

Comments
 (0)