Diff: SDSK.cpp

Revision:

1:491a39c644b1

Parent:

0:913deac5f975

Child:

2:ea603e66a32e

--- 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;