Set previous_nominal_speed to 0 if the next block is a zero move block so jerk can...

clean up some stuff

diff --git a/src/modules/robot/Conveyor.h b/src/modules/robot/Conveyor.h
index 186d805..f4ef6f4 100644 (file)
--- a/src/modules/robot/Conveyor.h
+++ b/src/modules/robot/Conveyor.h
@@ -14,25 +14,28 @@ using namespace std;
 #include <string>
 #include <vector>
 
-class Conveyor : public Module {
-    public:
-        Conveyor();
-
-        void on_module_loaded(void);
-        void on_idle(void*);
-
-        Block* new_block();
-        void new_block_added();
-        void pop_and_process_new_block(int debug);
-        void wait_for_queue(int free_blocks);
-        void wait_for_empty_queue();
-        bool is_queue_empty();
-
-        RingBuffer<Block,32> queue;  // Queue of Blocks
-        Block* current_block;
-        bool looking_for_new_block;
-
-        volatile int flush_blocks;
+class Conveyor : public Module
+{
+public:
+    Conveyor();
+
+    void on_module_loaded(void);
+    void on_idle(void *);
+
+    Block *new_block();
+    void new_block_added();
+    void pop_and_process_new_block(int debug);
+    void wait_for_queue(int free_blocks);
+    void wait_for_empty_queue();
+    bool is_queue_empty();
+
+    // right now block queue size can only be changed at compile time by changing the value below
+    typedef RingBuffer<Block, 32> BlockQueue_t;
+    BlockQueue_t queue;  // Queue of Blocks
+    Block *current_block;
+    bool looking_for_new_block;
+
+    volatile int flush_blocks;
 };
 
 #endif // CONVEYOR_H

diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp
index 58de4eb..3a8a4ae 100644 (file)
--- a/src/modules/robot/Planner.cpp
+++ b/src/modules/robot/Planner.cpp
@@ -64,7 +64,7 @@ void Planner::append_block( int target[], float feed_rate, float distance, float
 
     block->millimeters = distance;
     float inverse_millimeters = 0.0F;
-    if( distance > 0 ){ inverse_millimeters = 1.0/distance; }
+    if( distance > 0 ){ inverse_millimeters = 1.0F/distance; }
 
     // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
     // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
@@ -103,7 +103,7 @@ void Planner::append_block( int target[], float feed_rate, float distance, float
     // nonlinearities of both the junction angle and junction velocity.
     float vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
 
-    if (THEKERNEL->conveyor->queue.size() > 1 && (this->previous_nominal_speed > 0.0)) {
+    if (THEKERNEL->conveyor->queue.size() > 1 && (this->previous_nominal_speed > 0.0F)) {
       // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
       // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
       float cos_theta = - this->previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
@@ -111,21 +111,21 @@ void Planner::append_block( int target[], float feed_rate, float distance, float
                          - this->previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
 
       // Skip and use default max junction speed for 0 degree acute junction.
-      if (cos_theta < 0.95) {
+      if (cos_theta < 0.95F) {
         vmax_junction = min(this->previous_nominal_speed,block->nominal_speed);
         // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
-        if (cos_theta > -0.95) {
+        if (cos_theta > -0.95F) {
           // Compute maximum junction velocity based on maximum acceleration and junction deviation
-          float sin_theta_d2 = sqrtf(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
+          float sin_theta_d2 = sqrtf(0.5F*(1.0F-cos_theta)); // Trig half angle identity. Always positive.
           vmax_junction = min(vmax_junction,
-            sqrtf(this->acceleration * this->junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
+            sqrtf(this->acceleration * this->junction_deviation * sin_theta_d2/(1.0F-sin_theta_d2)) );
         }
       }
     }
     block->max_entry_speed = vmax_junction;
 
     // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
-    float v_allowable = this->max_allowable_speed(-this->acceleration,0.0,block->millimeters); //TODO: Get from config
+    float v_allowable = this->max_allowable_speed(-this->acceleration,0.0F,block->millimeters); //TODO: Get from config
     block->entry_speed = min(vmax_junction, v_allowable);
 
     // Initialize planner efficiency flags
@@ -174,7 +174,7 @@ void Planner::append_block( int target[], float feed_rate, float distance, float
 // 3. Recalculate trapezoids for all blocks.
 //
 void Planner::recalculate() {
-    RingBuffer<Block,32>* queue = &THEKERNEL->conveyor->queue;
+    Conveyor::BlockQueue_t *queue = &THEKERNEL->conveyor->queue;
 
     int newest = queue->prev_block_index(queue->head);
     int oldest = queue->tail;
@@ -246,7 +246,7 @@ void Planner::dump_queue(){
 // acceleration within the allotted distance.
 float Planner::max_allowable_speed(float acceleration, float target_velocity, float distance) {
   return(
-    sqrtf(target_velocity*target_velocity-2L*acceleration*distance)  //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
+    sqrtf(target_velocity*target_velocity-2.0F*acceleration*distance)  //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
   );
 }
 

diff --git a/src/modules/robot/Planner.h b/src/modules/robot/Planner.h
index 6136190..60cbb8b 100644 (file)
--- a/src/modules/robot/Planner.h
+++ b/src/modules/robot/Planner.h
@@ -18,7 +18,7 @@
 #define junction_deviation_checksum CHECKSUM("junction_deviation")
 
 // TODO: Get from config
-#define MINIMUM_PLANNER_SPEED 0.0
+#define MINIMUM_PLANNER_SPEED 0.0F
 using namespace std;
 
 

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 63a0d3a..a0e8857 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -406,7 +406,12 @@ void Robot::append_line(Gcode* gcode, float target[], float rate ){
     gcode->millimeters_of_travel = sqrtf( pow( target[X_AXIS]-this->current_position[X_AXIS], 2 ) +  pow( target[Y_AXIS]-this->current_position[Y_AXIS], 2 ) +  pow( target[Z_AXIS]-this->current_position[Z_AXIS], 2 ) );
 
     // We ignore non-moves ( for example, extruder moves are not XYZ moves )
-    if( gcode->millimeters_of_travel < 0.0001 ){ return; }
+    if( gcode->millimeters_of_travel < 0.0001 ){
+        // an extruder only move means we stopped so we need to tell planner that previous speed and unitvector are zero
+        THEKERNEL->planner->previous_nominal_speed = 0;
+        clear_vector_float(THEKERNEL->planner->previous_unit_vec);
+        return;
+    }
 
     // Mark the gcode as having a known distance
     this->distance_in_gcode_is_known( gcode );