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

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