#include #include #include "StepGen3.h" #include "extend32to64.h" extern "C" { #include "esc.h" } //////////////////////////////////////////////// /******************************************************************** * Description: stepgen.c * This file, 'stepgen.c', is a HAL component that * provides software based step pulse generation. * * Author: John Kasunich * License: GPL Version 2 * * Copyright (c) 2003-2007 All rights reserved. * * Last change: ********************************************************************/ /** This file, 'stepgen.c', is a HAL component that provides software based step pulse generation. The maximum step rate will depend on the speed of the PC, but is expected to exceed 5KHz for even the slowest computers, and may reach 25KHz on fast ones. It is a realtime component. It supports up to 16 pulse generators. Each generator can produce several types of outputs in addition to step/dir, including quadrature, half- and full-step unipolar and bipolar, three phase, and five phase. A 32 bit feedback value is provided indicating the current position of the motor in counts (assuming no lost steps), and a floating point feedback in user specified position units is also provided. The number of step generators and type of outputs is determined by the insmod command line parameter 'step_type'. It accepts a comma separated (no spaces) list of up to 16 stepping types to configure up to 16 channels. A second command line parameter "ctrl_type", selects between position and velocity control modes for each step generator. (ctrl_type is optional, the default control type is position.) So a command line like this: insmod stepgen step_type=0,0,1,2 ctrl_type=p,p,v,p will install four step generators, two using stepping type 0, one using type 1, and one using type 2. The first two and the last one will be running in position mode, and the third one will be running in velocity mode. The driver exports three functions. 'stepgen.make-pulses', is responsible for actually generating the step pulses. It must be executed in a fast thread to reduce pulse jitter. The other two functions are normally called from a much slower thread. 'stepgen.update-freq' reads the position or frequency command and sets internal variables used by 'stepgen.make-pulses'. 'stepgen.capture-position' captures and scales the current values of the position feedback counters. Both 'update-freq' and 'capture-position' use floating point, 'make-pulses' does not. Polarity: All signals from this module have fixed polarity (active high in most cases). If the driver needs the opposite polarity, the signals can be inverted using parameters exported by the hardware driver(s) such as ParPort. Timing parameters: There are five timing parameters which control the output waveform. No step type uses all five, and only those which will be used are exported to HAL. The values of these parameters are in nano-seconds, so no recalculation is needed when changing thread periods. In the timing diagrams that follow, they are identified by the following numbers: (1): 'stepgen.n.steplen' = length of the step pulse (2): 'stepgen.n.stepspace' = minimum space between step pulses (actual space depends on frequency command, and is infinite if the frequency command is zero) (3): 'stepgen.n.dirhold' = minimum delay after a step pulse before a direction change - may be longer (4): 'stepgen.n.dirsetup' = minimum delay after a direction change and before the next step - may be longer (5): 'stepgen.n.dirdelay' = minimum delay after a step before a step in the opposite direction - may be longer Stepping Types: This module supports a number of stepping types, as follows: Type 0: Step and Direction _____ _____ _____ STEP ____/ \_______/ \_____________/ \______ | | | | | | Time |-(1)-|--(2)--|-(1)-|--(3)--|-(4)-|-(1)-| |__________________ DIR ________________________________/ There are two output pins, STEP and DIR. Step pulses appear on STEP. A positive frequency command results in DIR low, negative frequency command means DIR high. The minimum period of the step pulses is 'steplen' + 'stepspace', and the frequency command is clamped to avoid exceeding these limits. 'steplen' and 'stepspace' must both be non-zero. 'dirsetup' or 'dirhold' may be zero, but their sum must be non-zero, to ensure non-zero low time between the last up step and the first down step. Type 1: Up/Down _____ _____ UP __/ \_____/ \________________________________ | | | | | Time |-(1)-|-(2)-|-(1)-|---(5)---|-(1)-|-(2)-|-(1)-| |_____| |_____| DOWN ______________________________/ \_____/ \____ There are two output pins, UP and DOWN. A positive frequency command results in pulses on UP, negative frequency command results in pulses on DOWN. The minimum period of the step pulses is 'steplen' + 'stepspace', and the frequency command is clamped to avoid exceeding these limits. 'steplen', 'stepspace', and 'dirdelay' must all be non-zero. Types 2 and higher: State Patterns STATE |---1---|---2---|---3---|----4----|---3---|---2---| | | | | | | | Time |--(1)--|--(1)--|--(1)--|--(1+5)--|--(1)--|--(1)--| All the remaining stepping types are simply different repeating patterns on two to five output pins. When a step occurs, the output pins change to the next (or previous) pattern in the state listings that follow. The output pins are called 'PhaseA' thru 'PhaseE'. Timing constraints are obeyed as indicated in the drawing above. 'steplen' must be non-zero. 'dirdelay' may be zero. Because stepspace is not used, state based stepping types can run faster than types 0 and 1. Type 2: Quadrature (aka Gray/Grey code) State Phase A Phase B 0 1 0 1 1 1 2 0 1 3 0 0 0 1 0 Type 3: Three Wire State Phase A Phase B Phase C 0 1 0 0 1 0 1 0 2 0 0 1 0 1 0 0 Type 4: Three Wire HalfStep State Phase A Phase B Phase C 0 1 0 0 1 1 1 0 2 0 1 0 3 0 1 1 4 0 0 1 5 1 0 1 0 1 0 0 Type 5: Unipolar Full Step (one winding on) State Phase A Phase B Phase C Phase D 0 1 0 0 0 1 0 1 0 0 2 0 0 1 0 3 0 0 0 1 0 1 0 0 0 Type 6: Unipolar Full Step (two windings on) State Phase A Phase B Phase C Phase D 0 1 1 0 0 1 0 1 1 0 2 0 0 1 1 3 1 0 0 1 0 1 1 0 0 Type 7: Bipolar Full Step (one winding on) State Phase A Phase B Phase C Phase D 0 1 0 0 0 1 1 1 1 0 2 0 1 1 1 3 0 0 0 1 0 1 0 0 0 Type 8: Bipolar Full Step (two windings on) State Phase A Phase B Phase C Phase D 0 1 0 1 0 1 0 1 1 0 2 0 1 0 1 3 1 0 0 1 0 1 0 1 0 Type 9: Unipolar Half Step State Phase A Phase B Phase C Phase D 0 1 0 0 0 1 1 1 0 0 2 0 1 0 0 3 0 1 1 0 4 0 0 1 0 5 0 0 1 1 6 0 0 0 1 7 1 0 0 1 0 1 0 0 0 Type 10: Bipolar Half Step State Phase A Phase B Phase C Phase D 0 1 0 0 0 1 1 0 1 0 2 1 1 1 0 3 0 1 1 0 4 0 1 1 1 5 0 1 0 1 6 0 0 0 1 7 1 0 0 1 0 1 0 0 0 Type 11: Five Wire Unipolar State Phase A Phase B Phase C Phase D Phase E 0 1 0 0 0 0 1 0 1 0 0 0 2 0 0 1 0 0 3 0 0 0 1 0 4 0 0 0 0 1 0 1 0 0 0 0 Type 12: Five Wire Wave State Phase A Phase B Phase C Phase D Phase E 0 1 1 0 0 0 1 0 1 1 0 0 2 0 0 1 1 0 3 0 0 0 1 1 4 1 0 0 0 1 0 1 1 0 0 0 Type 13: Five Wire Unipolar HalfStep State Phase A Phase B Phase C Phase D Phase E 0 1 0 0 0 0 1 1 1 0 0 0 2 0 1 0 0 0 3 0 1 1 0 0 4 0 0 1 0 0 5 0 0 1 1 0 6 0 0 0 1 0 7 0 0 0 1 1 8 0 0 0 0 1 9 1 0 0 0 1 0 1 0 0 0 0 Type 14: Five Wire Wave HalfStep State Phase A Phase B Phase C Phase D Phase E 0 1 1 0 0 0 1 1 1 1 0 0 2 0 1 1 0 0 3 0 1 1 1 0 4 0 0 1 1 0 5 0 0 1 1 1 6 0 0 0 1 1 7 1 0 0 1 1 8 1 0 0 0 1 9 1 1 0 0 1 0 1 1 0 0 0 */ /** This program is free software; you can redistribute it and/or modify it under the terms of version 2 of the GNU General Public License as published by the Free Software Foundation. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this library; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR ANY HARM OR LOSS RESULTING FROM ITS USE. IT IS _EXTREMELY_ UNWISE TO RELY ON SOFTWARE ALONE FOR SAFETY. Any machinery capable of harming persons must have provisions for completely removing power from all motors, etc, before persons enter any danger area. All machinery must be designed to comply with local and national safety codes, and the authors of this software can not, and do not, take any responsibility for such compliance. This code was written as part of the EMC HAL project. For more information, go to www.linuxcnc.org. */ #include "StepGen3.h" #include #include #include #include using namespace std; /*********************************************************************** * STRUCTURES AND GLOBAL VARIABLES * ************************************************************************/ /** This structure contains the runtime data for a single generator. */ /* structure members are ordered to optimize caching for makepulses, which runs in the fastest thread */ /* ptr to array of stepgen_t structs in shared memory, 1 per channel */ /*********************************************************************** * LOCAL FUNCTION DECLARATIONS * ************************************************************************/ /*********************************************************************** * REALTIME STEP PULSE GENERATION FUNCTIONS * ************************************************************************/ /** The frequency generator works by adding a signed value proportional to frequency to an accumulator. When bit PICKOFF of the accumulator toggles, a step is generated. */ void StepGen3::make_pulses(void *arg, long period) { stepgen_t *stepgen; long old_addval, target_addval, new_addval, step_now; int n, p; unsigned char outbits; /* store period so scaling constants can be (re)calculated */ periodns = period; /* point to stepgen data structures */ stepgen = (stepgen_t *)arg; cnt++; for (n = 0; n < num_chan; n++) { /* decrement "timing constraint" timers */ if (stepgen->timer1 > 0) { if (stepgen->timer1 > periodns) { stepgen->timer1 -= periodns; } else { stepgen->timer1 = 0; } } if (stepgen->timer2 > 0) { if (stepgen->timer2 > periodns) { stepgen->timer2 -= periodns; } else { stepgen->timer2 = 0; } } if (stepgen->timer3 > 0) { if (stepgen->timer3 > periodns) { stepgen->timer3 -= periodns; } else { stepgen->timer3 = 0; /* last timer timed out, cancel hold */ stepgen->hold_dds = 0; } } if (!stepgen->hold_dds && stepgen->enable) { /* update addval (ramping) */ old_addval = stepgen->addval; target_addval = stepgen->target_addval; if (stepgen->deltalim != 0) { /* implement accel/decel limit */ if (target_addval > (old_addval + stepgen->deltalim)) { /* new value is too high, increase addval as far as possible */ new_addval = old_addval + stepgen->deltalim; } else if (target_addval < (old_addval - stepgen->deltalim)) { /* new value is too low, decrease addval as far as possible */ new_addval = old_addval - stepgen->deltalim; } else { /* new value can be reached in one step - do it */ new_addval = target_addval; } } else { /* go to new freq without any ramping */ new_addval = target_addval; } /* save result */ stepgen->addval = new_addval; /* check for direction reversal */ if (((new_addval >= 0) && (old_addval < 0)) || ((new_addval < 0) && (old_addval >= 0))) { /* reversal required, can we do so now? */ if (stepgen->timer3 != 0) { /* no - hold everything until delays time out */ stepgen->hold_dds = 1; } } } /* update DDS */ if (!stepgen->hold_dds && stepgen->enable) { /* save current value of low half of accum */ step_now = stepgen->accum; /* update the accumulator */ stepgen->accum += stepgen->addval; /* test for changes in low half of accum */ step_now ^= stepgen->accum; /* we only care about the pickoff bit */ step_now &= (1L << PICKOFF); /* update rawcounts parameter */ stepgen->rawcount = stepgen->accum >> PICKOFF; } else { /* DDS is in hold, no steps */ step_now = 0; } if (stepgen->timer2 == 0) { /* update direction - do not change if addval = 0 */ if (stepgen->addval > 0) { stepgen->curr_dir = 1; } else if (stepgen->addval < 0) { stepgen->curr_dir = -1; } } if (step_now) { /* (re)start various timers */ /* timer 1 = time till end of step pulse */ stepgen->timer1 = stepgen->step_len; /* timer 2 = time till allowed to change dir pin */ stepgen->timer2 = stepgen->timer1 + stepgen->dir_hold_dly; /* timer 3 = time till allowed to step the other way */ stepgen->timer3 = stepgen->timer2 + stepgen->dir_setup; if (stepgen->step_type >= 2) { /* update state */ stepgen->state += stepgen->curr_dir; if (stepgen->state < 0) { stepgen->state = stepgen->cycle_max; } else if (stepgen->state > stepgen->cycle_max) { stepgen->state = 0; } } } /* generate output, based on stepping type */ if (stepgen->step_type == 0) { /* step/dir output */ if (stepgen->timer1 != 0) { stepgen->phase[STEP_PIN] = 1; } else { stepgen->phase[STEP_PIN] = 0; } if (stepgen->curr_dir < 0) { stepgen->phase[DIR_PIN] = 1; } else { stepgen->phase[DIR_PIN] = 0; } } else if (stepgen->step_type == 1) { /* up/down */ if (stepgen->timer1 != 0) { if (stepgen->curr_dir < 0) { stepgen->phase[UP_PIN] = 0; stepgen->phase[DOWN_PIN] = 1; } else { stepgen->phase[UP_PIN] = 1; stepgen->phase[DOWN_PIN] = 0; } } else { stepgen->phase[UP_PIN] = 0; stepgen->phase[DOWN_PIN] = 0; } } else { /* step type 2 or greater */ /* look up correct output pattern */ outbits = (stepgen->lut)[stepgen->state]; /* now output the phase bits */ for (p = 0; p < stepgen->num_phases; p++) { /* output one phase */ stepgen->phase[p] = outbits & 1; /* move to the next phase */ outbits >>= 1; } } /* move on to next step generator */ stepgen++; } /* done */ } void StepGen3::update_pos(void *arg, long period) { long long int accum_a, accum_b; stepgen_t *stepgen; int n; stepgen = (stepgen_t *)arg; for (n = 0; n < num_chan; n++) { /* 'accum' is a long long, and its remotely possible that make_pulses could change it half-way through a read. So we have a crude atomic read routine */ do { accum_a = stepgen->accum; accum_b = stepgen->accum; } while (accum_a != accum_b); /* compute integer counts */ stepgen->count = accum_a >> PICKOFF; /* check for change in scale value */ if (stepgen->pos_scale != stepgen->old_scale) { /* get ready to detect future scale changes */ stepgen->old_scale = stepgen->pos_scale; /* validate the new scale value */ if ((stepgen->pos_scale < 1e-20) && (stepgen->pos_scale > -1e-20)) { /* value too small, divide by zero is a bad thing */ stepgen->pos_scale = 1.0; } /* we will need the reciprocal, and the accum is fixed point with fractional bits, so we precalc some stuff */ stepgen->scale_recip = (1.0 / (1L << PICKOFF)) / stepgen->pos_scale; } /* scale accumulator to make floating point position, after removing the one-half count offset */ stepgen->pos_fb = (double)(accum_a - (1 << (PICKOFF - 1))) * stepgen->scale_recip; /* move on to next channel */ stepgen++; } /* done */ } /* helper function - computes integral multiple of increment that is greater or equal to value */ unsigned long StepGen3::ulceil(unsigned long value, unsigned long increment) { if (value == 0) { return 0; } return increment * (((value - 1) / increment) + 1); } void StepGen3::update_freq(void *arg, long period) { stepgen_t *stepgen; int n, newperiod; long min_step_period; long long int accum_a, accum_b; double pos_cmd, vel_cmd, curr_pos, curr_vel, avg_v, max_freq, max_ac; double match_ac, match_time, est_out, est_cmd, est_err, dp, dv, new_vel; double desired_freq; /*! \todo FIXME - while this code works just fine, there are a bunch of internal variables, many of which hold intermediate results that don't really need their own variables. They are used either for clarity, or because that's how the code evolved. This algorithm could use some cleanup and optimization. */ /* this periodns stuff is a little convoluted because we need to calculate some constants here in this relatively slow thread but the constants are based on the period of the much faster 'make_pulses()' thread. */ /* only recalc constants if period changes */ newperiod = 0; if (periodns != old_periodns) { /* get ready to detect future period changes */ old_periodns = periodns; /* recompute various constants that depend on periodns */ periodfp = periodns * 0.000000001; freqscale = (1L << PICKOFF) * periodfp; accelscale = freqscale * periodfp; /* force re-evaluation of the timing parameters */ newperiod = 1; } /* now recalc constants related to the period of this funct */ /* only recalc constants if period changes */ if (period != old_dtns) { /* get ready to detect future period changes */ old_dtns = period; /* dT is the period of this thread, used for the position loop */ dt = period * 0.000000001; /* calc the reciprocal once here, to avoid multiple divides later */ recip_dt = 1.0 / dt; } /* point at stepgen data */ stepgen = (stepgen_t *)arg; /* loop thru generators */ for (n = 0; n < num_chan; n++) { /* check for scale change */ if (stepgen->pos_scale != stepgen->old_scale) { /* get ready to detect future scale changes */ stepgen->old_scale = stepgen->pos_scale; /* validate the new scale value */ if ((stepgen->pos_scale < 1e-20) && (stepgen->pos_scale > -1e-20)) { /* value too small, divide by zero is a bad thing */ stepgen->pos_scale = 1.0; } /* we will need the reciprocal, and the accum is fixed point with fractional bits, so we precalc some stuff */ stepgen->scale_recip = (1.0 / (1L << PICKOFF)) / stepgen->pos_scale; } if (newperiod) { /* period changed, force recalc of timing parameters */ stepgen->old_step_len = ~0; stepgen->old_step_space = ~0; stepgen->old_dir_hold_dly = ~0; stepgen->old_dir_setup = ~0; } /* process timing parameters */ if (stepgen->step_len != stepgen->old_step_len) { /* must be non-zero */ if (stepgen->step_len == 0) { stepgen->step_len = 1; } /* make integer multiple of periodns */ stepgen->old_step_len = ulceil(stepgen->step_len, periodns); stepgen->step_len = stepgen->old_step_len; } if (stepgen->step_space != stepgen->old_step_space) { /* make integer multiple of periodns */ stepgen->old_step_space = ulceil(stepgen->step_space, periodns); stepgen->step_space = stepgen->old_step_space; } if (stepgen->dir_setup != stepgen->old_dir_setup) { /* make integer multiple of periodns */ stepgen->old_dir_setup = ulceil(stepgen->dir_setup, periodns); stepgen->dir_setup = stepgen->old_dir_setup; } if (stepgen->dir_hold_dly != stepgen->old_dir_hold_dly) { if ((stepgen->dir_hold_dly + stepgen->dir_setup) == 0) { /* dirdelay must be non-zero step types 0 and 1 */ if (stepgen->step_type < 2) { stepgen->dir_hold_dly = 1; } } stepgen->old_dir_hold_dly = ulceil(stepgen->dir_hold_dly, periodns); stepgen->dir_hold_dly = stepgen->old_dir_hold_dly; } /* test for disabled stepgen */ if (stepgen->enable == 0) { /* disabled: keep updating old_pos_cmd (if in pos ctrl mode) */ if (stepgen->pos_mode) { stepgen->old_pos_cmd = stepgen->pos_cmd * stepgen->pos_scale; } /* set velocity to zero */ stepgen->freq = 0; stepgen->addval = 0; stepgen->target_addval = 0; /* and skip to next one */ stepgen++; continue; } /* calculate frequency limit */ min_step_period = stepgen->step_len + stepgen->step_space; max_freq = 1.0 / (min_step_period * 0.000000001); /* check for user specified frequency limit parameter */ if (stepgen->maxvel <= 0.0) { /* set to zero if negative */ stepgen->maxvel = 0.0; } else { /* parameter is non-zero, compare to max_freq */ desired_freq = stepgen->maxvel * fabs(stepgen->pos_scale); if (desired_freq > max_freq) { /* parameter is too high, complain about it */ if (!stepgen->printed_error) { printf("STEPGEN: Channel %d: The requested maximum velocity of %d steps/sec is too high.\n", n, (int)desired_freq); printf("STEPGEN: The maximum possible frequency is %d steps/second\n", (int)max_freq); stepgen->printed_error = 1; } /* parameter is too high, limit it */ stepgen->maxvel = max_freq / fabs(stepgen->pos_scale); } else { /* lower max_freq to match parameter */ max_freq = stepgen->maxvel * fabs(stepgen->pos_scale); } } /* set internal accel limit to its absolute max, which is zero to full speed in one thread period */ max_ac = max_freq * recip_dt; /* check for user specified accel limit parameter */ if (stepgen->maxaccel <= 0.0) { /* set to zero if negative */ stepgen->maxaccel = 0.0; } else { /* parameter is non-zero, compare to max_ac */ if ((stepgen->maxaccel * fabs(stepgen->pos_scale)) > max_ac) { /* parameter is too high, lower it */ stepgen->maxaccel = max_ac / fabs(stepgen->pos_scale); } else { /* lower limit to match parameter */ max_ac = stepgen->maxaccel * fabs(stepgen->pos_scale); } } /* at this point, all scaling, limits, and other parameter changes have been handled - time for the main control */ if (stepgen->pos_mode) { /* calculate position command in counts */ pos_cmd = stepgen->pos_cmd * stepgen->pos_scale; /* calculate velocity command in counts/sec */ vel_cmd = (pos_cmd - stepgen->old_pos_cmd) * recip_dt; stepgen->old_pos_cmd = pos_cmd; /* 'accum' is a long long, and its remotely possible that make_pulses could change it half-way through a read. So we have a crude atomic read routine */ do { accum_a = stepgen->accum; accum_b = stepgen->accum; } while (accum_a != accum_b); /* convert from fixed point to double, after subtracting the one-half step offset */ curr_pos = (accum_a - (1 << (PICKOFF - 1))) * (1.0 / (1L << PICKOFF)); /* get velocity in counts/sec */ curr_vel = stepgen->freq; /* At this point we have good values for pos_cmd, curr_pos, vel_cmd, curr_vel, max_freq and max_ac, all in counts, counts/sec, or counts/sec^2. Now we just have to do something useful with them. */ /* determine which way we need to ramp to match velocity */ if (vel_cmd > curr_vel) { match_ac = max_ac; } else { match_ac = -max_ac; } /* determine how long the match would take */ match_time = (vel_cmd - curr_vel) / match_ac; /* calc output position at the end of the match */ avg_v = (vel_cmd + curr_vel) * 0.5; est_out = curr_pos + avg_v * match_time; /* calculate the expected command position at that time */ est_cmd = pos_cmd + vel_cmd * (match_time - 1.5 * dt); /* calculate error at that time */ est_err = est_out - est_cmd; if (match_time < dt) { /* we can match velocity in one period */ if (fabs(est_err) < 0.0001) { /* after match the position error will be acceptable */ /* so we just do the velocity match */ new_vel = vel_cmd; } else { /* try to correct position error */ new_vel = vel_cmd - 0.5 * est_err * recip_dt; /* apply accel limits */ if (new_vel > (curr_vel + max_ac * dt)) { new_vel = curr_vel + max_ac * dt; } else if (new_vel < (curr_vel - max_ac * dt)) { new_vel = curr_vel - max_ac * dt; } } } else { /* calculate change in final position if we ramp in the opposite direction for one period */ dv = -2.0 * match_ac * dt; dp = dv * match_time; /* decide which way to ramp */ if (fabs(est_err + dp * 2.0) < fabs(est_err)) { match_ac = -match_ac; } /* and do it */ new_vel = curr_vel + match_ac * dt; } /* apply frequency limit */ if (new_vel > max_freq) { new_vel = max_freq; } else if (new_vel < -max_freq) { new_vel = -max_freq; } /* end of position mode */ } else { /* velocity mode is simpler */ /* calculate velocity command in counts/sec */ vel_cmd = stepgen->vel_cmd * stepgen->pos_scale; /* apply frequency limit */ if (vel_cmd > max_freq) { vel_cmd = max_freq; } else if (vel_cmd < -max_freq) { vel_cmd = -max_freq; } /* calc max change in frequency in one period */ dv = max_ac * dt; /* apply accel limit */ if (vel_cmd > (stepgen->freq + dv)) { new_vel = stepgen->freq + dv; } else if (vel_cmd < (stepgen->freq - dv)) { new_vel = stepgen->freq - dv; } else { new_vel = vel_cmd; } /* end of velocity mode */ } stepgen->freq = new_vel; /* calculate new addval */ stepgen->target_addval = stepgen->freq * freqscale; /* calculate new deltalim */ stepgen->deltalim = max_ac * accelscale; /* move on to next channel */ stepgen++; } /* done */ } /*********************************************************************** * LOCAL FUNCTION DEFINITIONS * ************************************************************************/ int StepGen3::export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode) { int n, retval, msg; /* This function exports a lot of stuff, which results in a lot of logging if msg_level is at INFO or ALL. So we save the current value of msg_level and restore it later. If you actually need to log this function's actions, change the second line below */ /* export output pins */ if (step_type == 0) { addr->phase[STEP_PIN] = 0; addr->phase[DIR_PIN] = 0; } else if (step_type == 1) { addr->phase[UP_PIN] = 0; addr->phase[DOWN_PIN] = 0; } else { /* stepping types 2 and higher use a varying number of phase pins */ addr->num_phases = num_phases_lut[step_type - 2]; for (n = 0; n < addr->num_phases; n++) { addr->phase[n] = 0; } } printf("Exporting %d\n", num); /* set default parameter values */ addr->pos_scale = 1.0; addr->old_scale = 0.0; addr->scale_recip = 0.0; addr->freq = 0.0; addr->maxvel = 0.0; addr->maxaccel = 0.0; addr->step_type = step_type; addr->pos_mode = pos_mode; printf("Exporting 1\n"); /* timing parameter defaults depend on step type */ addr->step_len = 1; if (step_type < 2) { addr->step_space = 1; } else { addr->step_space = 0; } if (step_type == 0) { addr->dir_hold_dly = 1; addr->dir_setup = 1; } else { addr->dir_hold_dly = 1; addr->dir_setup = 0; } /* set 'old' values to make update_freq validate the timing params */ printf("Exporting 2\n"); addr->old_step_len = ~0; addr->old_step_space = ~0; addr->old_dir_hold_dly = ~0; addr->old_dir_setup = ~0; if (step_type >= 2) { /* init output stuff */ addr->cycle_max = cycle_len_lut[step_type - 2] - 1; addr->lut = &(master_lut[step_type - 2][0]); } /* init the step generator core to zero output */ addr->timer1 = 0; addr->timer2 = 0; addr->timer3 = 0; addr->hold_dds = 0; addr->addval = 0; /* accumulator gets a half step offset, so it will step half way between integer positions, not at the integer positions */ addr->accum = 1 << (PICKOFF - 1); addr->rawcount = 0; addr->curr_dir = 0; addr->state = 0; addr->enable = 0; addr->target_addval = 0; addr->deltalim = 0; /* other init */ addr->printed_error = 0; addr->old_pos_cmd = 0.0; printf("Exporting 3\n"); /* set initial pin values */ addr->count = 0; addr->pos_fb = 0.0; if (pos_mode) { addr->pos_cmd = 0.0; } else { addr->vel_cmd = 0.0; } /* restore saved message level */ // rtapi_set_msg_level(msg); printf("Exporting done\n"); return 0; } int StepGen3::setup_user_step_type(void) { int used_phases = 0; int i = 0; for (i = 0; i < MAX_CYCLE && user_step_type[i] != -1; i++) { master_lut[USER_STEP_TYPE][i] = user_step_type[i]; used_phases |= user_step_type[i]; } cycle_len_lut[USER_STEP_TYPE] = i; if (used_phases & ~0x1f) { printf("STEPGEN: ERROR: bad user step type uses more than 5 phases"); return -1; // EINVAL more than 5 phases is not allowed } if (used_phases & 0x10) num_phases_lut[USER_STEP_TYPE] = 5; else if (used_phases & 0x8) num_phases_lut[USER_STEP_TYPE] = 4; else if (used_phases & 0x4) num_phases_lut[USER_STEP_TYPE] = 3; else if (used_phases & 0x2) num_phases_lut[USER_STEP_TYPE] = 2; else if (used_phases & 0x1) num_phases_lut[USER_STEP_TYPE] = 1; if (used_phases) printf("User step type has %d phases and %d steps per cycle\n", num_phases_lut[USER_STEP_TYPE], i); return 0; } CONTROL StepGen3::parse_ctrl_type(const char *ctrl) { if (!ctrl || !*ctrl || *ctrl == 'p' || *ctrl == 'P') return POSITION; if (*ctrl == 'v' || *ctrl == 'V') return VELOCITY; return INVALID; } StepGen3::StepGen3(void) { step_type[0] = 0; ctrl_type[0] = (char *)malloc(2); strcpy(ctrl_type[0], "p"); stepPin[0] = PA11; dirPin[0] = PA12; step_type[1] = 0; ctrl_type[1] = (char *)malloc(2); strcpy(ctrl_type[1], "p"); stepPin[1] = PC9; dirPin[1] = PC10; rtapi_app_main(); stepgen_array[0].enable = 1; stepgen_array[0].pos_scale = JOINT_X_SCALE; stepgen_array[0].maxaccel = JOINT_X_STEPGEN_MAXACCEL; stepgen_array[1].pos_scale = JOINT_Z_SCALE; stepgen_array[1].maxaccel = JOINT_Z_STEPGEN_MAXACCEL; stepgen_array[0].enable = stepgen_array[1].enable = 1; } void StepGen3::updateStepGen(double pos_cmd1, double pos_cmd2, uint32_t servoPeriod) { stepgen_array[0].pos_cmd = pos_cmd1; stepgen_array[1].pos_cmd = pos_cmd2; for (int i = 0; i < num_chan; i++) { stepgen_t *step; step = &(stepgen_array[i]); update_pos(step, servoPeriod); // servoPeriod is in nanosecs update_freq(step, servoPeriod); } } int StepGen3::rtapi_app_main() { int n, retval; retval = setup_user_step_type(); if (retval < 0) { return retval; } for (n = 0; n < MAX_CHAN && step_type[n] != -1; n++) { if ((step_type[n] > MAX_STEP_TYPE) || (step_type[n] < 0)) { printf("STEPGEN: ERROR: bad stepping type '%i', axis %i\n", step_type[n], n); return -1; } if (parse_ctrl_type(ctrl_type[n]) == INVALID) { printf("STEPGEN: ERROR: bad control type '%s' for axis %i (must be 'p' or 'v')\n", ctrl_type[n], n); return -1; } num_chan++; } if (num_chan == 0) { printf("STEPGEN: ERROR: no channels configured\n"); return -1; } /* periodns will be set to the proper value when 'make_pulses()' runs for the first time. We load a default value here to avoid glitches at startup, but all these 'constants' are recomputed inside 'update_freq()' using the real period. */ old_periodns = periodns = 50000; old_dtns = 1000000; /* precompute some constants */ periodfp = periodns * 0.000000001; freqscale = (1L << PICKOFF) * periodfp; accelscale = freqscale * periodfp; dt = old_dtns * 0.000000001; recip_dt = 1.0 / dt; /* have good config info */ /* allocate shared memory for counter data */ stepgen_array = (stepgen_t *)malloc(num_chan * sizeof(stepgen_t)); if (stepgen_array == 0) { printf("STEPGEN: ERROR: hal_malloc() failed\n"); return -1; } /* export all the variables for each pulse generator */ for (n = 0; n < num_chan; n++) { /* export all vars */ retval = export_stepgen(n, &(stepgen_array[n]), step_type[n], (parse_ctrl_type(ctrl_type[n]) == POSITION)); if (retval != 0) { printf("STEPGEN: ERROR: stepgen %d var export failed\n", n); return -1; } } return 0; } extern volatile uint64_t makePulsesCnt; void StepGen3::makeAllPulses(void) { makePulsesCnt++; make_pulses(stepgen_array, BASE_PERIOD); for (int i = 0; i < num_chan; i++) { digitalWrite(dirPin[i], stepgen_array[i].phase[DIR_PIN] ? LOW : HIGH); digitalWrite(stepPin[i], stepgen_array[i].phase[STEP_PIN] ? HIGH : LOW); } }