File content as of revision 1:491a39c644b1:

#include "SDSK.h"

SDSK::SDSK(PinName _pin_step, PinName _pin_dir, uint32_t _steps_per_rev, double _max_acc, double _max_vel, uint32_t _min_us_step_pulse, bool _positive_is_ccw)
:step(_pin_step), dir(_pin_dir),
positive_is_ccw(_positive_is_ccw),
max_acc(_max_acc), max_vel(_max_vel),
min_us_step_pulse(_min_us_step_pulse),
position_angle(0),
steps_per_rev(_steps_per_rev),
steps_per_angle(_steps_per_rev/360.0),
pc(USBTX, USBRX),
profile_time()
{
    profile_time.start();
}
void SDSK::reverse()
{
    positive_is_ccw = !positive_is_ccw;
}
void SDSK::zero()
{
    position_angle = 0;
}
void SDSK::move(double target_angle)
{
    // set_direction
    //pc.printf("current:%f\ttarget:%f\n", position_angle, target_angle);
    
    if (target_angle > position_angle)
        dir = positive_is_ccw;
    else
        dir = !positive_is_ccw;
    
    // convert degree parameters into steps    
    uint64_t steps_to_take = abs(target_angle - position_angle)*steps_per_angle;
        
    uint32_t max_acc_steps = max_acc*steps_per_angle; // steps/sec^2
    uint32_t max_vel_steps = max_vel*steps_per_angle; // steps/sec
    
    // bound variables.  0's don't make sense (and cause undefined math)
    if (max_acc_steps < 1)
        max_acc_steps = 1;
    if (max_vel_steps < 1)
        max_vel_steps = 1;
    
    // how much time to get up to full velocity: t = v/a.
    double seconds_til_max_vel = (double) max_vel_steps / max_acc_steps;
    
    // Then use that time at constant acc to find the steps taken in that time: steps = 1/2*a*t^2 
    uint64_t steps_to_max_vel = 0.5*max_acc_steps*(seconds_til_max_vel)*(seconds_til_max_vel);        
    
    // CREATE PROFILE ///////////////
    uint64_t steps_accelerating;
    uint64_t steps_decelerating; 
    uint64_t steps_at_max_vel;
    
    
    uint32_t vel_reached_steps;
    
        
    // if we'll never reach max velocity, make triangle profile (not trapezoidal)
    if (steps_to_max_vel >= (steps_to_take / 2.0))
    {
        // round down on decelerating steps
        steps_decelerating = steps_to_take/2;
        steps_accelerating = steps_to_take - steps_decelerating;
        steps_at_max_vel = 0;
        
        double time_for_steps = sqrt((double)steps_to_take/max_acc_steps);        
        vel_reached_steps = max_acc_steps * time_for_steps;
    }
    else
    {
        steps_decelerating = steps_to_max_vel;
        steps_accelerating = steps_to_max_vel;
        steps_at_max_vel = steps_to_take - steps_decelerating - steps_accelerating;
        
        vel_reached_steps = max_vel_steps;
    }
    
    
    //pc.printf("steps_acc:%llu\t", steps_accelerating);
    //pc.printf("steps_at_max_vel:%llu\t", steps_at_max_vel);
    //pc.printf("steps_decelerating:%llu\n", steps_decelerating);
    
    // RUN PROFILE ///////////////
    
    // steps = 1/2*a*t^2 + v*t + steps0
    // let t0 = 0, steps0 = 0, acceleration = a
    // steps = 1/2*a*t*t
    // t = sqrt(2*steps/a)
    // t[s] = sqrt(2*steps/a)
    // t[us] = 10^6*sqrt(2*steps/a)
    profile_time.reset();
    for (uint64_t i=0; i<steps_accelerating; i++)
    {
        //uint64_t trigger_time = 1000000*sqrt(2.0*(i)/max_acc_steps);
        uint64_t trigger_time = 1000000 * sqrt((2.0/max_acc_steps)*i);
        //pc.printf("A\t%d\t%llu\n", i, trigger_time);
        while (profile_time.read_high_resolution_us() < trigger_time);
        take_step();
    }
    
    
    // steps = 1/2*a*t^2 + v*t + steps0
    // let t0 = 0, steps0 = 0, acceleration = 0
    // steps = v*t
    // t = steps/v
    // t[s] = steps/v
    // t[us] = 10^6*steps/v
    profile_time.reset();
    for (uint64_t i=0; i<steps_at_max_vel; i++)
    {        
        uint64_t trigger_time = 1000000.0*i/vel_reached_steps;
        //pc.printf("C\t%d\t%llu\n", i, trigger_time);
        while (profile_time.read_high_resolution_us() < trigger_time);
        
        take_step();
    }
    
    // steps = 1/2*a*t^2 + v*t + steps0
    // let t0 = 0, steps0 = 0, acceleration = -a
    // steps = -1/2*a*t^2 + v*t
    // 0 = -1/2*a*t^2 + v*t - steps
    // quadratic formula:
    // t = [-b ± sqrt(b^2-4*a*c)]/(2*a)
    // t = [-v0 ± sqrt(v0^2 - 4*(-1/2*a)*(-steps))]/(2*-1/2*a)
    // t = [-v0 ± sqrt(v0^2 - 4*(-1/2*a)*(-steps))]/(-a)
    // t = [-v0 ± sqrt(v0^2 - 4*(1/2*a)*(steps))]/(-a)
    // t = [-v0 ± sqrt(v0^2 - 2*(a)*(steps))]/(-a)
    // t = v0/a ± -1/a*sqrt(v0^2 - 2*(a)*(steps))
    // p1 := v0/a
    // p2 := 1/a
    // p3 := sqrt(v0^2 - 2*(a)*(steps))
    // t = p1 - p2*p3
    // t[s] = p1 - p2*p3
    // t[us] = 10^6*p1 - 10^6*p2*p3
    profile_time.reset();
    for (uint64_t i=0; i<steps_decelerating; i++)
    {
        uint64_t p1 = 1000000.0*vel_reached_steps/max_acc_steps;
        //double p2 = (1000000.0/max_acc_steps);
        uint64_t p3 = 1000000.0 * sqrt(((double)vel_reached_steps/max_acc_steps)*((double)vel_reached_steps/max_acc_steps) -2.0*i/((double)max_acc_steps));
        uint64_t trigger_time = (p1 - p3);
        
        //pc.printf("D\t%d\t%llu\n", i, trigger_time); 
        while (profile_time.read_high_resolution_us() < trigger_time);
        take_step();
    }
    
    position_angle = target_angle;
}
void SDSK::take_step()
{
    step = 1;
    wait_us(min_us_step_pulse);
    step = 0;
}

void SDSK::set_max_acc(double _max_acc)
{
    max_acc = _max_acc;
}
void SDSK::set_max_vel(double _max_vel)
{
    max_vel = _max_vel;
}