A Clearpath SDSK trapezoidal motion controller
Revision 1:491a39c644b1, committed 2018-04-05
- Comitter:
- williamweatherholtz
- Date:
- Thu Apr 05 19:12:24 2018 +0000
- Parent:
- 0:913deac5f975
- Commit message:
- a Clearpath SDSK trapezoidal control implementation
Changed in this revision
SDSK.cpp | Show annotated file Show diff for this revision Revisions of this file |
SDSK.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 913deac5f975 -r 491a39c644b1 SDSK.cpp --- a/SDSK.cpp Thu Feb 01 19:06:08 2018 +0000 +++ b/SDSK.cpp Thu Apr 05 19:12:24 2018 +0000 @@ -36,7 +36,7 @@ // 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 ROUNDED UP + 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) @@ -56,6 +56,7 @@ uint64_t steps_decelerating; uint64_t steps_at_max_vel; + uint32_t vel_reached_steps; @@ -74,11 +75,16 @@ { steps_decelerating = steps_to_max_vel; steps_accelerating = steps_to_max_vel; - steps_at_max_vel = steps_to_take - steps_decelerating - steps_accelerating; + 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 @@ -90,8 +96,9 @@ profile_time.reset(); for (uint64_t i=0; i<steps_accelerating; i++) { - uint64_t trigger_time = 1000000*sqrt(2.0*(i)/max_acc_steps); - //pc.printf("%lu, ", trigger_time); + //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(); } @@ -105,9 +112,9 @@ // 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; + { + 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(); @@ -130,31 +137,21 @@ // 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; - uint64_t p2 = (1000000.0/max_acc_steps); - double p3 = sqrt((double)(vel_reached_steps*vel_reached_steps) -2.0*((double)max_acc_steps)*(i)); + //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); - uint64_t trigger_time = (p1 - p2*p3); - - while (profile_time.read_high_resolution_us() < trigger_time); + //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::delay_for_velocity(double velocity_steps) -{ - uint32_t delay = 1000000.0/velocity_steps; - - if (delay < min_us_step_pulse) - delay = min_us_step_pulse; - - wait_us(delay); -} void SDSK::take_step() { step = 1;
diff -r 913deac5f975 -r 491a39c644b1 SDSK.h --- a/SDSK.h Thu Feb 01 19:06:08 2018 +0000 +++ b/SDSK.h Thu Apr 05 19:12:24 2018 +0000 @@ -6,28 +6,20 @@ class SDSK { public: - SDSK(PinName _pin_step, PinName _pin_dir, uint32_t _steps_per_rev=51200, double _max_acc=200, double _max_vel=180, uint32_t _min_us_step_pulse=4, bool _positive_is_ccw=true); + SDSK(PinName _pin_step, PinName _pin_dir, uint32_t _steps_per_rev=51200, double _max_acc=360, double _max_vel=180, uint32_t _min_us_step_pulse=4, bool _positive_is_ccw=true); void move(double target_angle); - void reverse(); void zero(); void set_max_acc(double _max_acc); void set_max_vel(double _max_vel); - void take_step(); - - //void set_direction(bool cw); + void take_step(); private: DigitalOut step; DigitalOut dir; - - Timer profile_time; - - Serial pc; - bool positive_is_ccw; // changes to -1 if untrue double max_acc; @@ -39,6 +31,9 @@ uint32_t steps_per_rev; double steps_per_angle; - void delay_for_velocity(double velocity_steps); + Serial pc; + + Timer profile_time; + }; #endif