William Weatherholtz / SDSK

A Clearpath SDSK trapezoidal motion controller

SDSK.cpp@1:491a39c644b1, 2018-04-05 (annotated)

Committer:
williamweatherholtz
Date:
Thu Apr 05 19:12:24 2018 +0000
Revision:
1:491a39c644b1
Parent:
0:913deac5f975 
a Clearpath SDSK trapezoidal control implementation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
williamweatherholtz 0:913deac5f975 1
williamweatherholtz 0:913deac5f975 2
williamweatherholtz 0:913deac5f975 3 #include "SDSK.h"
williamweatherholtz 0:913deac5f975 4
williamweatherholtz 0:913deac5f975 5 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)
williamweatherholtz 0:913deac5f975 6 :step(_pin_step), dir(_pin_dir),
williamweatherholtz 0:913deac5f975 7 positive_is_ccw(_positive_is_ccw),
williamweatherholtz 0:913deac5f975 8 max_acc(_max_acc), max_vel(_max_vel),
williamweatherholtz 0:913deac5f975 9 min_us_step_pulse(_min_us_step_pulse),
williamweatherholtz 0:913deac5f975 10 position_angle(0),
williamweatherholtz 0:913deac5f975 11 steps_per_rev(_steps_per_rev),
williamweatherholtz 0:913deac5f975 12 steps_per_angle(_steps_per_rev/360.0),
williamweatherholtz 0:913deac5f975 13 pc(USBTX, USBRX),
williamweatherholtz 0:913deac5f975 14 profile_time()
williamweatherholtz 0:913deac5f975 15 {
williamweatherholtz 0:913deac5f975 16 profile_time.start();
williamweatherholtz 0:913deac5f975 17 }
williamweatherholtz 0:913deac5f975 18 void SDSK::reverse()
williamweatherholtz 0:913deac5f975 19 {
williamweatherholtz 0:913deac5f975 20 positive_is_ccw = !positive_is_ccw;
williamweatherholtz 0:913deac5f975 21 }
williamweatherholtz 0:913deac5f975 22 void SDSK::zero()
williamweatherholtz 0:913deac5f975 23 {
williamweatherholtz 0:913deac5f975 24 position_angle = 0;
williamweatherholtz 0:913deac5f975 25 }
williamweatherholtz 0:913deac5f975 26 void SDSK::move(double target_angle)
williamweatherholtz 0:913deac5f975 27 {
williamweatherholtz 0:913deac5f975 28 // set_direction
williamweatherholtz 0:913deac5f975 29 //pc.printf("current:%f\ttarget:%f\n", position_angle, target_angle);
williamweatherholtz 0:913deac5f975 30
williamweatherholtz 0:913deac5f975 31 if (target_angle > position_angle)
williamweatherholtz 0:913deac5f975 32 dir = positive_is_ccw;
williamweatherholtz 0:913deac5f975 33 else
williamweatherholtz 0:913deac5f975 34 dir = !positive_is_ccw;
williamweatherholtz 0:913deac5f975 35
williamweatherholtz 0:913deac5f975 36 // convert degree parameters into steps
williamweatherholtz 0:913deac5f975 37 uint64_t steps_to_take = abs(target_angle - position_angle)*steps_per_angle;
williamweatherholtz 0:913deac5f975 38
williamweatherholtz 1:491a39c644b1 39 uint32_t max_acc_steps = max_acc*steps_per_angle; // steps/sec^2
williamweatherholtz 0:913deac5f975 40 uint32_t max_vel_steps = max_vel*steps_per_angle; // steps/sec
williamweatherholtz 0:913deac5f975 41
williamweatherholtz 0:913deac5f975 42 // bound variables. 0's don't make sense (and cause undefined math)
williamweatherholtz 0:913deac5f975 43 if (max_acc_steps < 1)
williamweatherholtz 0:913deac5f975 44 max_acc_steps = 1;
williamweatherholtz 0:913deac5f975 45 if (max_vel_steps < 1)
williamweatherholtz 0:913deac5f975 46 max_vel_steps = 1;
williamweatherholtz 0:913deac5f975 47
williamweatherholtz 0:913deac5f975 48 // how much time to get up to full velocity: t = v/a.
williamweatherholtz 0:913deac5f975 49 double seconds_til_max_vel = (double) max_vel_steps / max_acc_steps;
williamweatherholtz 0:913deac5f975 50
williamweatherholtz 0:913deac5f975 51 // Then use that time at constant acc to find the steps taken in that time: steps = 1/2*a*t^2
williamweatherholtz 0:913deac5f975 52 uint64_t steps_to_max_vel = 0.5*max_acc_steps*(seconds_til_max_vel)*(seconds_til_max_vel);
williamweatherholtz 0:913deac5f975 53
williamweatherholtz 0:913deac5f975 54 // CREATE PROFILE ///////////////
williamweatherholtz 0:913deac5f975 55 uint64_t steps_accelerating;
williamweatherholtz 0:913deac5f975 56 uint64_t steps_decelerating;
williamweatherholtz 0:913deac5f975 57 uint64_t steps_at_max_vel;
williamweatherholtz 0:913deac5f975 58
williamweatherholtz 1:491a39c644b1 59
williamweatherholtz 0:913deac5f975 60 uint32_t vel_reached_steps;
williamweatherholtz 0:913deac5f975 61
williamweatherholtz 0:913deac5f975 62
williamweatherholtz 0:913deac5f975 63 // if we'll never reach max velocity, make triangle profile (not trapezoidal)
williamweatherholtz 0:913deac5f975 64 if (steps_to_max_vel >= (steps_to_take / 2.0))
williamweatherholtz 0:913deac5f975 65 {
williamweatherholtz 0:913deac5f975 66 // round down on decelerating steps
williamweatherholtz 0:913deac5f975 67 steps_decelerating = steps_to_take/2;
williamweatherholtz 0:913deac5f975 68 steps_accelerating = steps_to_take - steps_decelerating;
williamweatherholtz 0:913deac5f975 69 steps_at_max_vel = 0;
williamweatherholtz 0:913deac5f975 70
williamweatherholtz 0:913deac5f975 71 double time_for_steps = sqrt((double)steps_to_take/max_acc_steps);
williamweatherholtz 0:913deac5f975 72 vel_reached_steps = max_acc_steps * time_for_steps;
williamweatherholtz 0:913deac5f975 73 }
williamweatherholtz 0:913deac5f975 74 else
williamweatherholtz 0:913deac5f975 75 {
williamweatherholtz 0:913deac5f975 76 steps_decelerating = steps_to_max_vel;
williamweatherholtz 0:913deac5f975 77 steps_accelerating = steps_to_max_vel;
williamweatherholtz 1:491a39c644b1 78 steps_at_max_vel = steps_to_take - steps_decelerating - steps_accelerating;
williamweatherholtz 0:913deac5f975 79
williamweatherholtz 0:913deac5f975 80 vel_reached_steps = max_vel_steps;
williamweatherholtz 0:913deac5f975 81 }
williamweatherholtz 0:913deac5f975 82
williamweatherholtz 1:491a39c644b1 83
williamweatherholtz 1:491a39c644b1 84 //pc.printf("steps_acc:%llu\t", steps_accelerating);
williamweatherholtz 1:491a39c644b1 85 //pc.printf("steps_at_max_vel:%llu\t", steps_at_max_vel);
williamweatherholtz 1:491a39c644b1 86 //pc.printf("steps_decelerating:%llu\n", steps_decelerating);
williamweatherholtz 1:491a39c644b1 87
williamweatherholtz 0:913deac5f975 88 // RUN PROFILE ///////////////
williamweatherholtz 0:913deac5f975 89
williamweatherholtz 0:913deac5f975 90 // steps = 1/2*a*t^2 + v*t + steps0
williamweatherholtz 0:913deac5f975 91 // let t0 = 0, steps0 = 0, acceleration = a
williamweatherholtz 0:913deac5f975 92 // steps = 1/2*a*t*t
williamweatherholtz 0:913deac5f975 93 // t = sqrt(2*steps/a)
williamweatherholtz 0:913deac5f975 94 // t[s] = sqrt(2*steps/a)
williamweatherholtz 0:913deac5f975 95 // t[us] = 10^6*sqrt(2*steps/a)
williamweatherholtz 0:913deac5f975 96 profile_time.reset();
williamweatherholtz 0:913deac5f975 97 for (uint64_t i=0; i<steps_accelerating; i++)
williamweatherholtz 0:913deac5f975 98 {
williamweatherholtz 1:491a39c644b1 99 //uint64_t trigger_time = 1000000*sqrt(2.0*(i)/max_acc_steps);
williamweatherholtz 1:491a39c644b1 100 uint64_t trigger_time = 1000000 * sqrt((2.0/max_acc_steps)*i);
williamweatherholtz 1:491a39c644b1 101 //pc.printf("A\t%d\t%llu\n", i, trigger_time);
williamweatherholtz 0:913deac5f975 102 while (profile_time.read_high_resolution_us() < trigger_time);
williamweatherholtz 0:913deac5f975 103 take_step();
williamweatherholtz 0:913deac5f975 104 }
williamweatherholtz 0:913deac5f975 105
williamweatherholtz 0:913deac5f975 106
williamweatherholtz 0:913deac5f975 107 // steps = 1/2*a*t^2 + v*t + steps0
williamweatherholtz 0:913deac5f975 108 // let t0 = 0, steps0 = 0, acceleration = 0
williamweatherholtz 0:913deac5f975 109 // steps = v*t
williamweatherholtz 0:913deac5f975 110 // t = steps/v
williamweatherholtz 0:913deac5f975 111 // t[s] = steps/v
williamweatherholtz 0:913deac5f975 112 // t[us] = 10^6*steps/v
williamweatherholtz 0:913deac5f975 113 profile_time.reset();
williamweatherholtz 0:913deac5f975 114 for (uint64_t i=0; i<steps_at_max_vel; i++)
williamweatherholtz 1:491a39c644b1 115 {
williamweatherholtz 1:491a39c644b1 116 uint64_t trigger_time = 1000000.0*i/vel_reached_steps;
williamweatherholtz 1:491a39c644b1 117 //pc.printf("C\t%d\t%llu\n", i, trigger_time);
williamweatherholtz 0:913deac5f975 118 while (profile_time.read_high_resolution_us() < trigger_time);
williamweatherholtz 0:913deac5f975 119
williamweatherholtz 0:913deac5f975 120 take_step();
williamweatherholtz 0:913deac5f975 121 }
williamweatherholtz 0:913deac5f975 122
williamweatherholtz 0:913deac5f975 123 // steps = 1/2*a*t^2 + v*t + steps0
williamweatherholtz 0:913deac5f975 124 // let t0 = 0, steps0 = 0, acceleration = -a
williamweatherholtz 0:913deac5f975 125 // steps = -1/2*a*t^2 + v*t
williamweatherholtz 0:913deac5f975 126 // 0 = -1/2*a*t^2 + v*t - steps
williamweatherholtz 0:913deac5f975 127 // quadratic formula:
williamweatherholtz 0:913deac5f975 128 // t = [-b ± sqrt(b^2-4*a*c)]/(2*a)
williamweatherholtz 0:913deac5f975 129 // t = [-v0 ± sqrt(v0^2 - 4*(-1/2*a)*(-steps))]/(2*-1/2*a)
williamweatherholtz 0:913deac5f975 130 // t = [-v0 ± sqrt(v0^2 - 4*(-1/2*a)*(-steps))]/(-a)
williamweatherholtz 0:913deac5f975 131 // t = [-v0 ± sqrt(v0^2 - 4*(1/2*a)*(steps))]/(-a)
williamweatherholtz 0:913deac5f975 132 // t = [-v0 ± sqrt(v0^2 - 2*(a)*(steps))]/(-a)
williamweatherholtz 0:913deac5f975 133 // t = v0/a ± -1/a*sqrt(v0^2 - 2*(a)*(steps))
williamweatherholtz 0:913deac5f975 134 // p1 := v0/a
williamweatherholtz 0:913deac5f975 135 // p2 := 1/a
williamweatherholtz 0:913deac5f975 136 // p3 := sqrt(v0^2 - 2*(a)*(steps))
williamweatherholtz 0:913deac5f975 137 // t = p1 - p2*p3
williamweatherholtz 0:913deac5f975 138 // t[s] = p1 - p2*p3
williamweatherholtz 0:913deac5f975 139 // t[us] = 10^6*p1 - 10^6*p2*p3
williamweatherholtz 0:913deac5f975 140 profile_time.reset();
williamweatherholtz 0:913deac5f975 141 for (uint64_t i=0; i<steps_decelerating; i++)
williamweatherholtz 0:913deac5f975 142 {
williamweatherholtz 0:913deac5f975 143 uint64_t p1 = 1000000.0*vel_reached_steps/max_acc_steps;
williamweatherholtz 1:491a39c644b1 144 //double p2 = (1000000.0/max_acc_steps);
williamweatherholtz 1:491a39c644b1 145 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));
williamweatherholtz 1:491a39c644b1 146 uint64_t trigger_time = (p1 - p3);
williamweatherholtz 0:913deac5f975 147
williamweatherholtz 1:491a39c644b1 148 //pc.printf("D\t%d\t%llu\n", i, trigger_time);
williamweatherholtz 1:491a39c644b1 149 while (profile_time.read_high_resolution_us() < trigger_time);
williamweatherholtz 0:913deac5f975 150 take_step();
williamweatherholtz 0:913deac5f975 151 }
williamweatherholtz 0:913deac5f975 152
williamweatherholtz 0:913deac5f975 153 position_angle = target_angle;
williamweatherholtz 0:913deac5f975 154 }
williamweatherholtz 0:913deac5f975 155 void SDSK::take_step()
williamweatherholtz 0:913deac5f975 156 {
williamweatherholtz 0:913deac5f975 157 step = 1;
williamweatherholtz 0:913deac5f975 158 wait_us(min_us_step_pulse);
williamweatherholtz 0:913deac5f975 159 step = 0;
williamweatherholtz 0:913deac5f975 160 }
williamweatherholtz 0:913deac5f975 161
williamweatherholtz 0:913deac5f975 162 void SDSK::set_max_acc(double _max_acc)
williamweatherholtz 0:913deac5f975 163 {
williamweatherholtz 0:913deac5f975 164 max_acc = _max_acc;
williamweatherholtz 0:913deac5f975 165 }
williamweatherholtz 0:913deac5f975 166 void SDSK::set_max_vel(double _max_vel)
williamweatherholtz 0:913deac5f975 167 {
williamweatherholtz 0:913deac5f975 168 max_vel = _max_vel;
williamweatherholtz 0:913deac5f975 169 }