Revision 2:ea603e66a32e, committed 2019-12-02

Comitter:

lippiello

Date:

Mon Dec 02 11:38:14 2019 +0000

Parent:

1:491a39c644b1

Commit message:

Nuova versione con le mie modifiche;

Changed in this revision

--- a/SDSK.cpp	Thu Apr 05 19:12:24 2018 +0000
+++ b/SDSK.cpp	Mon Dec 02 11:38:14 2019 +0000
@@ -10,6 +10,7 @@
 position_angle(0),
 steps_per_rev(_steps_per_rev),
 steps_per_angle(_steps_per_rev/360.0),
+angle_per_steps(360.0/(double)_steps_per_rev),
 pc(USBTX, USBRX),
 profile_time()
 {
@@ -23,15 +24,10 @@
 {
     position_angle = 0;
 }
-void SDSK::move(double target_angle)
+double 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;
+    set_dir(target_angle);
     
     // convert degree parameters into steps    
     uint64_t steps_to_take = abs(target_angle - position_angle)*steps_per_angle;
@@ -81,9 +77,9 @@
     }
     
     
-    //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);
+    //if (DEBUG) printf("steps_acc:%llu\t", steps_accelerating);
+    //if (DEBUG) printf("steps_at_max_vel:%llu\t", steps_at_max_vel);
+    //if (DEBUG) printf("steps_decelerating:%llu\n", steps_decelerating);
     
     // RUN PROFILE ///////////////
     
@@ -93,15 +89,18 @@
     // t = sqrt(2*steps/a)
     // t[s] = sqrt(2*steps/a)
     // t[us] = 10^6*sqrt(2*steps/a)
+    double time = 0;
     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);
+        time = sqrt((2.0/max_acc_steps)*i);
+        uint64_t trigger_time = 1000000 * time;
+        //if (DEBUG) printf("A\t%d\t%llu\n", i, trigger_time);
         while (profile_time.read_high_resolution_us() < trigger_time);
         take_step();
     }
+    double duration = time;
     
     
     // steps = 1/2*a*t^2 + v*t + steps0
@@ -113,12 +112,13 @@
     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);
+        time = i/vel_reached_steps;
+        uint64_t trigger_time = 1000000.0*time;
+        //if (DEBUG) printf("C\t%d\t%llu\n", i, trigger_time);
         while (profile_time.read_high_resolution_us() < trigger_time);
-        
         take_step();
     }
+    duration += time;
     
     // steps = 1/2*a*t^2 + v*t + steps0
     // let t0 = 0, steps0 = 0, acceleration = -a
@@ -140,23 +140,26 @@
     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); 
+        uint64_t p1 = vel_reached_steps/max_acc_steps;
+        uint64_t p3 = sqrt(((double)vel_reached_steps/max_acc_steps)*((double)vel_reached_steps/max_acc_steps) -2.0*i/((double)max_acc_steps));
+        time = p1 - p3;
+        uint64_t trigger_time = 1000000.0 * time;
+        //if (DEBUG) 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;
+    duration += time;
+    return duration;
 }
 void SDSK::take_step()
 {
     step = 1;
     wait_us(min_us_step_pulse);
     step = 0;
+    if (pos_dir)
+        position_angle += angle_per_steps;
+    else
+        position_angle -= angle_per_steps;
 }
 
 void SDSK::set_max_acc(double _max_acc)
@@ -166,4 +169,151 @@
 void SDSK::set_max_vel(double _max_vel)
 {
     max_vel = _max_vel;
+}
+
+
+vector<uint64_t> SDSK::planner(double target_angle)
+{
+    vector<uint64_t> step_time;
+    
+    // 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;
+    }
+    
+    //if (DEBUG) printf("steps_acc:%llu\t", steps_accelerating);
+    //if (DEBUG) printf("steps_at_max_vel:%llu\t", steps_at_max_vel);
+    //if (DEBUG) printf("steps_decelerating:%llu\n", steps_decelerating);
+    
+    // COMPUTE PROFILE ///////////////
+    uint64_t trigger_time, trigger_time_0 = 0;
+    
+    // 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)
+    for (uint64_t i=0; i<steps_accelerating; i++)
+    {
+        trigger_time = 1000000 * sqrt((2.0/max_acc_steps)*i);
+        //if (DEBUG) printf("A\t%d\t%llu\n", i, trigger_time);
+        step_time.push_back(trigger_time);
+    }
+    trigger_time_0 = trigger_time;
+    
+    // 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
+    for (uint64_t i=0; i<steps_at_max_vel; i++)
+    {        
+        uint64_t trigger_time = 1000000.0*i/vel_reached_steps;
+        //if (DEBUG) printf("C\t%d\t%llu\n", i, trigger_time);
+        step_time.push_back(trigger_time_0+trigger_time);
+    }
+    trigger_time_0 = trigger_time;
+    
+    // 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
+    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);
+        step_time.push_back(trigger_time_0+trigger_time);
+    }
+    
+    return step_time;
+}
+
+
+void SDSK::move_planner(vector<uint64_t> step_time, double target_angle)
+{
+    // set_direction
+    set_dir(target_angle);
+   
+    // RUN PROFILE ///////////////
+    profile_time.reset();
+    for (uint64_t i=0; i<step_time.size(); i++)
+    {
+        while (profile_time.read_high_resolution_us() < step_time[i]);
+        take_step();
+    }
+}
+
+void SDSK::set_dir(double target_angle)
+{
+    // set_direction
+    //if (DEBUG) printf("current:%f\ttarget:%f\n", position_angle, target_angle);
+    if (target_angle >= position_angle)
+    {
+        dir = positive_is_ccw;
+        pos_dir = true;
+    }
+    else
+    {
+        pos_dir = false;
+        dir = !positive_is_ccw;
+    }
 }
\ No newline at end of file

--- a/SDSK.h	Thu Apr 05 19:12:24 2018 +0000
+++ b/SDSK.h	Mon Dec 02 11:38:14 2019 +0000
@@ -1,14 +1,25 @@
 #ifndef SDSK_h
 #define SDSK_h
 
+#ifndef M_PI
+#define M_PI           3.14159265358979323846
+#endif
+#define DEG2RAD M_PI / 180.0
+#define RAD2DEG 180.0 / M_PI
+
 #include <mbed.h>
+#include <vector>
 
 class SDSK
 {
 public:
     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);
+    double move(double target_angle);
+    
+    vector<uint64_t> planner(double target_angle);
+    void move_planner(vector<uint64_t> step_time, double target_angle);
+    void set_dir(double target_angle);
     
     void reverse();
     void zero();
@@ -16,6 +27,7 @@
     void set_max_vel(double _max_vel);
     
     void take_step(); 
+    double position_angle;
      
 private:
     DigitalOut step;
@@ -26,10 +38,12 @@
     double max_vel;
     
     uint32_t min_us_step_pulse;
-    double position_angle;
     
     uint32_t steps_per_rev;
     double steps_per_angle;
+    double angle_per_steps;
+    
+    bool pos_dir;
     
     Serial pc;