Skip to content
Browse files

add jog vel pin for config jog vel

  • Loading branch information...
1 parent c23631d commit d5575ee71cbdade2a3ef558318b484c139d83066 Hsuyao Tsai committed Jun 26, 2012
Showing with 22 additions and 11 deletions.
  1. +3 −3 src/emc/motion/control.c
  2. +19 −8 src/hal/components/wou_stepgen.c
View
6 src/emc/motion/control.c
@@ -895,8 +895,8 @@ static void process_probe_inputs(void)
case USB_STATUS_READY: // PROBE STATUS Clean
// deal with PROBE related status only
+ emcmotStatus->align_pos_cmd = 0;
if (wait_resume == 1 && emcmotStatus->probe_cmd == USB_CMD_STATUS_ACK) {
- emcmotStatus->align_pos_cmd = 0;
fprintf(stderr,"controlc.: call tpResume()\n");
tpResume(&emcmotDebug->coord_tp);
wait_resume = 0;
@@ -908,7 +908,7 @@ static void process_probe_inputs(void)
emcmotStatus->probe_cmd = USB_CMD_NOOP;
if (probe_suppress == 0) { // just stop motion
tpPause(&emcmotDebug->coord_tp);
- reportError("G38.X probe move finished without tripping probe");
+ // reportError("G38.X probe move finished without tripping probe");
SET_MOTION_ERROR_FLAG(1);
}
}
@@ -924,7 +924,7 @@ static void process_probe_inputs(void)
if (probe_suppress == 0) { // just stop motion
tpPause(&emcmotDebug->coord_tp);
- reportError("G38.X probe move finished without tripping probe");
+ // reportError("G38.X probe move finished without tripping probe");
SET_MOTION_ERROR_FLAG(1);
}
View
27 src/hal/components/wou_stepgen.c
@@ -420,9 +420,11 @@ typedef struct {
hal_float_t *cmd_fbf; /* position command retained by RISC (cmd_fbs divided by scale) */
uint32_t jog_config; /* for risc jogging */
hal_float_t *jog_scale; /* for risc jogging */
+ hal_float_t *jog_vel;
+
hal_bit_t *jog_enable;
int8_t prev_jog_enable;
- double prev_jog_scale;
+ double prev_jog_vel;
// hal_bit_t *tp_enable; /* connect to joint jog active */
} stepgen_t;
// #pragma pack(pop) /* restore original alignment from stack */
@@ -1708,19 +1710,22 @@ static void update_freq(void *arg, long period)
enable = *stepgen->enable; // take enable status of first joint
for (n = 0; n < num_joints; n++) {
/* begin: handle jog config for RISC */
- if (*stepgen->jog_scale != stepgen->prev_jog_scale) {
+ if ((*stepgen->jog_vel) * (*stepgen->jog_scale) != stepgen->prev_jog_vel) {
+ double vel;
/* config jog setting */
- jog_var = (stepgen->jog_config & 0xFFF00000) >> 20; // fetch jog velocity
- jog_var = (uint32_t)(((double)jog_var) * (*stepgen->jog_scale));
+ vel = (*stepgen->jog_vel) * (*stepgen->jog_scale);
+ if (vel > stepgen->maxvel) {
+ vel = stepgen->maxvel;
+ }
+ jog_var = (uint32_t) (vel * (*stepgen->pos_scale_pin) * dt * FIXED_POINT_SCALE);
new_jog_config = (jog_var << 20) | (stepgen->jog_config & 0x000FFFFF);
new_jog_config = (new_jog_config & 0xFFF0FFFF);
new_jog_config |= (*stepgen->jog_enable) << 16;
write_mot_param (n, (JOG_CONFIG), new_jog_config);
- fprintf(stderr, "wou_stepgen.c: j (%d) jog-scale has been changed new jog_config(0x%0X)\n",
+ fprintf(stderr, "wou_stepgen.c: j (%d) jog-vel has been changed new jog_config(0x%0X)\n",
n, new_jog_config);
}
- stepgen->prev_jog_scale = *stepgen->jog_scale;
-
+ stepgen->prev_jog_vel = (*stepgen->jog_vel) * (*stepgen->jog_scale);
if (stepgen->prev_jog_enable != *stepgen->jog_enable) {
new_jog_config = (stepgen->jog_config & 0xFFF0FFFF);
new_jog_config |= (*stepgen->jog_enable) << 16;
@@ -2275,6 +2280,11 @@ static int export_stepgen(int num, stepgen_t * addr,/* obsolete: int step_type,*
return retval;
}
+ retval = hal_pin_float_newf(HAL_IN, &(addr->jog_vel), comp_id,
+ "wou.stepgen.%d.jog_vel", num);
+ if (retval != 0) {
+ return retval;
+ }
retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_enable), comp_id,
"wou.stepgen.%d.jog-enable", num);
@@ -2323,12 +2333,13 @@ static int export_stepgen(int num, stepgen_t * addr,/* obsolete: int step_type,*
(addr->accel_cmd) = 0.0;
*(addr->ctrl_type_switch) = 0;
*(addr->jog_scale) = 0.6;
- addr->prev_jog_scale = 1.0;
/* config jog setting */
max_pulse_tick = ((uint32_t)(max_vel * pos_scale * dt * FIXED_POINT_SCALE) + 1);
jog_config_value = strtoul(jog_config_str[num],NULL, 16);
jog_config_value &= 0x000FFFFF;
jog_config_value |= ((max_pulse_tick << 4) & 0xFFF00000); // ignore fraction part
+ *(addr->jog_vel) = max_vel * 0.5; // not make default jog vel too large
+ addr->prev_jog_vel = (*addr->jog_vel) * (*addr->jog_scale);
addr->jog_config = jog_config_value;
*addr->jog_enable = ((addr->jog_config) & 0x000F0000) >> 16;

0 comments on commit d5575ee

Please sign in to comment.
Something went wrong with that request. Please try again.