File content as of revision 2:a43e90cb271c:

#include "mbed.h"
#include "trapezoidalMotionCal.h"


trapezoidalMotionCal::trapezoidalMotionCal(double f_start, double f_stop, double f_max, double a_up, double a_down){
    _f_start = f_start;
    _f_stop  = f_stop;
    _f_max   = f_max;
    _a_up    = a_up;
    _a_down  = a_down;
    
    shortMode = 0;
    revMode = 0;
    
}

void trapezoidalMotionCal::setTarg(int32_t targ){
    
    targSteps = targ;
    
    if(targSteps < 0){
        revMode = 1;
        targSteps *= -1;
    }
    
    if (step_const <= 0) {
        shortMode = 1;
        step_accel = (2 * _a_down * targSteps + _f_start * _f_start - _f_stop * _f_stop) / (2 * (_a_down - _a_up));
        step_const = 0;
        step_decel = targSteps - step_accel;
        f_reach = sqrt(2 * _a_up * step_accel + _f_start * _f_start);
    }
    else{
        shortMode = 0;
        step_accel = (_f_max * _f_max - _f_start * _f_start) / (2 * _a_up);
        step_decel = (_f_stop * _f_stop - _f_max * _f_max) / (2 * _a_down);
        step_const = (double)targSteps - step_accel - step_decel;
    }
    
}

uint32_t trapezoidalMotionCal::calStepDelay(int32_t steps){
    uint32_t period_us;
    
    if (steps < step_accel) {
        period_us = 1000000 * pow(_f_start * _f_start + 2 * _a_up * steps, -0.5);
    }
    else if (steps < step_accel + step_const) {
        period_us = 1000000 / _f_max;
    }
    else {
        if (shortMode == 0) {
            period_us = 1000000 * pow(_f_max * _f_max + 2 * _a_down * (steps - step_accel - step_const), -0.5);
        }
        else {
            period_us = 1000000 * pow(f_reach * f_reach + 2 * _a_down * (steps - step_accel - step_const), -0.5);
        }
        
    }
    
    return period_us;
}