<commit_before>
//
//    Copyright (C) 2007-2009 <NAME>
//
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation; either version 2 of the License, or
//    (at your option) any later version.
//
//    This program 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 program; if not, write to the Free Software
//    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
//

#include <linux/slab.h>

#include "rtapi.h"
#include "rtapi_app.h"
#include "rtapi_string.h"
#include "rtapi_math.h"

#include "hal.h"

#include "hal/drivers/mesa-hostmot2/hostmot2.h"


#define f_period_s ((hal_float_t)(l_period_ns * 1e-9))




// 
// read accumulator to figure out where the stepper has gotten to
// 

void hm2_stepgen_process_tram_read(hostmot2_t *hm2, long l_period_ns) {
    int i;
    for (i = 0; i < hm2->stepgen.num_instances; i ++) {
        u32 acc = hm2->stepgen.accumulator_reg[i];
        s32 counts_delta;

        // those tricky users are always trying to get us to divide by zero
        if (fabs(hm2->stepgen.instance[i].hal.param.position_scale) < 1e-6) {
            if (hm2->stepgen.instance[i].hal.param.position_scale >= 0.0) {
                hm2->stepgen.instance[i].hal.param.position_scale = 1.0;
                HM2_ERR("stepgen %d position_scale is too close to 0, resetting to 1.0\n", i);
            } else {
                hm2->stepgen.instance[i].hal.param.position_scale = -1.0;
                HM2_ERR("stepgen %d position_scale is too close to 0, resetting to -1.0\n", i);
            }
        }

        // the HM2 Accumulator is a 16.16 bit fixed-point representation of the stepper position
        // the fractional part gives accurate velocity at low speeds
        // FIXME: should it be used to provide sub-step position feedback too?  Or is integral-step-only position feedback more accurate?
        counts_delta = (acc >> 16) - (hm2->stepgen.instance[i].prev_accumulator >> 16);
        if (counts_delta > 32768) {
            counts_delta -= 65536;
        } else if (counts_delta < -32768) {
            counts_delta += 65536;
        }
        *(hm2->stepgen.instance[i].hal.pin.counts) += counts_delta;
        hm2->stepgen.instance[i].counts_fractional = acc & 0xFFFF;
        *(hm2->stepgen.instance[i].hal.pin.position_fb) = (double)(*hm2->stepgen.instance[i].hal.pin.counts) / hm2->stepgen.instance[i].hal.param.position_scale;

        hm2->stepgen.instance[i].prev_accumulator = acc;
    }
}




static void hm2_stepgen_instance_prepare_tram_write(hostmot2_t *hm2, long l_period_ns, int i) {
    hal_float_t ff_vel;
    hal_float_t velocity_error;
    hal_float_t match_accel;
    hal_float_t seconds_to_vel_match;
    hal_float_t position_at_match;
    hal_float_t position_cmd_at_match;
    hal_float_t error_at_match;
    hal_float_t velocity_cmd;

    hal_float_t steps_per_sec_cmd;

    hm2_stepgen_instance_t *s = &hm2->stepgen.instance[i];


    //
    // first sanity-check our maxaccel and maxvel params
    //

    // maxvel must be >= 0.0, and may not be faster than 1 step per (steplen+stepspace) seconds
    {
        hal_float_t min_ns_per_step = s->hal.param.steplen + s->hal.param.stepspace;
        hal_float_t max_steps_per_s = 1.0e9 / min_ns_per_step;
        hal_float_t max_pos_per_s = max_steps_per_s / fabs(s->hal.param.position_scale);
        if (s->hal.param.maxvel > max_pos_per_s) {
            s->hal.param.maxvel = max_pos_per_s;
        } else if (s->hal.param.maxvel < 0.0) {
            HM2_ERR("stepgen.%02d.maxvel < 0, setting to its absolute value\n", i);
            s->hal.param.maxvel = fabs(s->hal.param.maxvel);
        } else if (s->hal.param.maxvel == 0.0) {
            s->hal.param.maxvel = max_pos_per_s;
        }
    }

    // maxaccel may not be negative
    if (s->hal.param.maxaccel < 0.0) {
        HM2_ERR("stepgen.%02d.maxaccel < 0, setting to its absolute value\n", i);
        s->hal.param.maxaccel = fabs(s->hal.param.maxaccel);
    }


    //
    // Here's the stepgen position controller.  It uses first-order
    // feedforward and proportional error feedback.  This code is based
    // on <NAME>'s software stepgen code.
    //

    // calculate feed-forward velocity in machine units per second
    ff_vel = (*s->hal.pin.position_cmd - s->old_position_cmd) / f_period_s;
    s->old_position_cmd = *s->hal.pin.position_cmd;

    velocity_error = *s->hal.pin.velocity_fb - ff_vel;

    // do we need to speed up or slow down?
    if (s->hal.param.maxaccel > 0) {
        // user has specified a maxaccel (in units/second)
        if (velocity_error > 0) {
            match_accel = -s->hal.param.maxaccel;
        } else {
            match_accel = s->hal.param.maxaccel;
        }
    } else {
        // maxaccel is 0, no limit, fix this velocity error by the next servo period!
        // this leaves acceleration control up to the trajectory planner
        if (velocity_error > 0) {
            match_accel = velocity_error / f_period_s;
        } else {
            match_accel = -velocity_error / f_period_s;
        }
    }

    if (match_accel == 0) {
        // vel is just right, dont need to accelerate
        seconds_to_vel_match = 0.0;
    } else {
        seconds_to_vel_match = (ff_vel - *s->hal.pin.velocity_fb) / match_accel;
    }

    // compute expected position at the time of velocity match
    {
        hal_float_t avg_v;
        avg_v = (ff_vel + *s->hal.pin.velocity_fb) * 0.5;
        position_at_match = *s->hal.pin.position_fb + (avg_v * seconds_to_vel_match);
    }

    position_cmd_at_match = *s->hal.pin.position_cmd + (ff_vel * seconds_to_vel_match);
    error_at_match = position_at_match - position_cmd_at_match;

    if (seconds_to_vel_match < f_period_s) {
        // we can match velocity in one period

        // the ".5/scale" is 1/2 of the step size in position units
        if (fabs(error_at_match) < (0.5 / s->hal.param.position_scale)) {
            velocity_cmd = ff_vel;
        } else {
            // try to correct position error
            velocity_cmd = ff_vel - (0.5 * error_at_match / f_period_s);
<commit_msg>commit "stepgen oscillation with negative scale" fix from <NAME><commit_after>
//
//    Copyright (C) 2007-2009 <NAME>
//
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation; either version 2 of the License, or
//    (at your option) any later version.
//
//    This program 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 program; if not, write to the Free Software
//    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
//

#include <linux/slab.h>

#include "rtapi.h"
#include "rtapi_app.h"
#include "rtapi_string.h"
#include "rtapi_math.h"

#include "hal.h"

#include "hal/drivers/mesa-hostmot2/hostmot2.h"


#define f_period_s ((hal_float_t)(l_period_ns * 1e-9))




// 
// read accumulator to figure out where the stepper has gotten to
// 

void hm2_stepgen_process_tram_read(hostmot2_t *hm2, long l_period_ns) {
    int i;
    for (i = 0; i < hm2->stepgen.num_instances; i ++) {
        u32 acc = hm2->stepgen.accumulator_reg[i];
        s32 counts_delta;

        // those tricky users are always trying to get us to divide by zero
        if (fabs(hm2->stepgen.instance[i].hal.param.position_scale) < 1e-6) {
            if (hm2->stepgen.instance[i].hal.param.position_scale >= 0.0) {
                hm2->stepgen.instance[i].hal.param.position_scale = 1.0;
                HM2_ERR("stepgen %d position_scale is too close to 0, resetting to 1.0\n", i);
            } else {
                hm2->stepgen.instance[i].hal.param.position_scale = -1.0;
                HM2_ERR("stepgen %d position_scale is too close to 0, resetting to -1.0\n", i);
            }
        }

        // the HM2 Accumulator is a 16.16 bit fixed-point representation of the stepper position
        // the fractional part gives accurate velocity at low speeds
        // FIXME: should it be used to provide sub-step position feedback too?  Or is integral-step-only position feedback more accurate?
        counts_delta = (acc >> 16) - (hm2->stepgen.instance[i].prev_accumulator >> 16);
        if (counts_delta > 32768) {
            counts_delta -= 65536;
        } else if (counts_delta < -32768) {
            counts_delta += 65536;
        }
        *(hm2->stepgen.instance[i].hal.pin.counts) += counts_delta;
        hm2->stepgen.instance[i].counts_fractional = acc & 0xFFFF;
        *(hm2->stepgen.instance[i].hal.pin.position_fb) = (double)(*hm2->stepgen.instance[i].hal.pin.counts) / hm2->stepgen.instance[i].hal.param.position_scale;

        hm2->stepgen.instance[i].prev_accumulator = acc;
    }
}




static void hm2_stepgen_instance_prepare_tram_write(hostmot2_t *hm2, long l_period_ns, int i) {
    hal_float_t ff_vel;
    hal_float_t velocity_error;
    hal_float_t match_accel;
    hal_float_t seconds_to_vel_match;
    hal_float_t position_at_match;
    hal_float_t position_cmd_at_match;
    hal_float_t error_at_match;
    hal_float_t velocity_cmd;

    hal_float_t steps_per_sec_cmd;

    hm2_stepgen_instance_t *s = &hm2->stepgen.instance[i];


    //
    // first sanity-check our maxaccel and maxvel params
    //

    // maxvel must be >= 0.0, and may not be faster than 1 step per (steplen+stepspace) seconds
    {
        hal_float_t min_ns_per_step = s->hal.param.steplen + s->hal.param.stepspace;
        hal_float_t max_steps_per_s = 1.0e9 / min_ns_per_step;
        hal_float_t max_pos_per_s = max_steps_per_s / fabs(s->hal.param.position_scale);
        if (s->hal.param.maxvel > max_pos_per_s) {
            s->hal.param.maxvel = max_pos_per_s;
        } else if (s->hal.param.maxvel < 0.0) {
            HM2_ERR("stepgen.%02d.maxvel < 0, setting to its absolute value\n", i);
            s->hal.param.maxvel = fabs(s->hal.param.maxvel);
        } else if (s->hal.param.maxvel == 0.0) {
            s->hal.param.maxvel = max_pos_per_s;
        }
    }

    // maxaccel may not be negative
    if (s->hal.param.maxaccel < 0.0) {
        HM2_ERR("stepgen.%02d.maxaccel < 0, setting to its absolute value\n", i);
        s->hal.param.maxaccel = fabs(s->hal.param.maxaccel);
    }


    //
    // Here's the stepgen position controller.  It uses first-order
    // feedforward and proportional error feedback.  This code is based
    // on <NAME>'s software stepgen code.
    //

    // calculate feed-forward velocity in machine units per second
    ff_vel = (*s->hal.pin.position_cmd - s->old_position_cmd) / f_period_s;
    s->old_position_cmd = *s->hal.pin.position_cmd;

    velocity_error = *s->hal.pin.velocity_fb - ff_vel;

    // do we need to speed up or slow down?
    if (s->hal.param.maxaccel > 0) {
        // user has specified a maxaccel (in units/second)
        if (velocity_error > 0) {
            match_accel = -s->hal.param.maxaccel;
        } else {
            match_accel = s->hal.param.maxaccel;
        }
    } else {
        // maxaccel is 0, no limit, fix this velocity error by the next servo period!
        // this leaves acceleration control up to the trajectory planner
        if (velocity_error > 0) {
            match_accel = velocity_error / f_period_s;
        } else {
            match_accel = -velocity_error / f_period_s;
        }
    }

    if (match_accel == 0) {
        // vel is just right, dont need to accelerate
        seconds_to_vel_match = 0.0;
    } else {
        seconds_to_vel_match = (ff_vel - *s->hal.pin.velocity_fb) / match_accel;
    }

    // compute expected position at the time of velocity match
    {
        hal_float_t avg_v;
        avg_v = (ff_vel + *s->hal.pin.velocity_fb) * 0.5;
        position_at_match = *s->hal.pin.position_fb + (avg_v * seconds_to_vel_match);
    }

    position_cmd_at_match = *s->hal.pin.position_cmd + (ff_vel * seconds_to_vel_match);
    error_at_match = position_at_match - position_cmd_at_match;

    if (seconds_to_vel_match < f_period_s) {
        // we can match velocity in one period

        // the ".5/scale" is 1/2 of the step size in position units
        if (fabs(error_at_match) < fabs(0.5 / s->hal.param.position_scale)) {
            velocity_cmd = ff_vel;
        } else {
            // try to correct position error
            velocity_cmd = ff_vel - (0.5 * error_at_match / f_period_s);

{'fraction_non_alphanumeric': 0.11008332696277043, 'fraction_numerical': 0.02018194327650791, 'mean_word_length': 2.4435377730981838, 'pattern_counts': {'":': 0, '<': 28, '<?xml version=': 0, '>': 112, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '962189', 'n_tokens_mistral': 4638, 'n_tokens_neox': 4374, 'n_words': 1142}