Allow TABS in config
[clinton/Smoothieware.git] / src / modules / robot / Robot.cpp
index 2aab755..f983baf 100644 (file)
@@ -7,16 +7,18 @@
 
 #include "libs/Module.h"
 #include "libs/Kernel.h"
+
+#include <math.h>
 #include <string>
 using std::string;
-#include <math.h>
+
 #include "Planner.h"
 #include "Conveyor.h"
 #include "Robot.h"
-#include "libs/nuts_bolts.h"
-#include "libs/Pin.h"
-#include "libs/StepperMotor.h"
-#include "../communication/utils/Gcode.h"
+#include "nuts_bolts.h"
+#include "Pin.h"
+#include "StepperMotor.h"
+#include "Gcode.h"
 #include "PublicDataRequest.h"
 #include "arm_solutions/BaseSolution.h"
 #include "arm_solutions/CartesianSolution.h"
@@ -24,6 +26,7 @@ using std::string;
 #include "arm_solutions/RostockSolution.h"
 #include "arm_solutions/JohannKosselSolution.h"
 #include "arm_solutions/HBotSolution.h"
+#include "StepTicker.h"
 
 #define  default_seek_rate_checksum          CHECKSUM("default_seek_rate")
 #define  default_feed_rate_checksum          CHECKSUM("default_feed_rate")
@@ -60,6 +63,11 @@ using std::string;
 #define  beta_steps_per_mm_checksum          CHECKSUM("beta_steps_per_mm")
 #define  gamma_steps_per_mm_checksum         CHECKSUM("gamma_steps_per_mm")
 
+#define  alpha_max_rate_checksum             CHECKSUM("alpha_max_rate")
+#define  beta_max_rate_checksum              CHECKSUM("beta_max_rate")
+#define  gamma_max_rate_checksum             CHECKSUM("gamma_max_rate")
+
+
 // new-style actuator stuff
 #define  actuator_checksum                   CHEKCSUM("actuator")
 
@@ -68,6 +76,7 @@ using std::string;
 #define  en_pin_checksum                     CHECKSUM("en_pin")
 
 #define  steps_per_mm_checksum               CHECKSUM("steps_per_mm")
+#define  max_rate_checksum                   CHECKSUM("max_rate")
 
 #define  alpha_checksum                      CHECKSUM("alpha")
 #define  beta_checksum                       CHECKSUM("beta")
@@ -83,10 +92,9 @@ Robot::Robot(){
     this->absolute_mode = true;
     this->motion_mode =  MOTION_MODE_SEEK;
     this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
-    clear_vector(this->current_position);
     clear_vector(this->last_milestone);
     this->arm_solution = NULL;
-    seconds_per_minute = 60.0;
+    seconds_per_minute = 60.0F;
 }
 
 //Called when the module has just been loaded
@@ -133,16 +141,16 @@ void Robot::on_config_reload(void* argument){
     }
 
 
-    this->feed_rate           = THEKERNEL->config->value(default_feed_rate_checksum   )->by_default(100    )->as_number() / 60;
-    this->seek_rate           = THEKERNEL->config->value(default_seek_rate_checksum   )->by_default(100    )->as_number() / 60;
-    this->mm_per_line_segment = THEKERNEL->config->value(mm_per_line_segment_checksum )->by_default(0.0f   )->as_number();
+    this->feed_rate           = THEKERNEL->config->value(default_feed_rate_checksum   )->by_default(  100.0F)->as_number();
+    this->seek_rate           = THEKERNEL->config->value(default_seek_rate_checksum   )->by_default(  100.0F)->as_number();
+    this->mm_per_line_segment = THEKERNEL->config->value(mm_per_line_segment_checksum )->by_default(    0.0F)->as_number();
     this->delta_segments_per_second = THEKERNEL->config->value(delta_segments_per_second_checksum )->by_default(0.0f   )->as_number();
-    this->mm_per_arc_segment  = THEKERNEL->config->value(mm_per_arc_segment_checksum  )->by_default(0.5f   )->as_number();
-    this->arc_correction      = THEKERNEL->config->value(arc_correction_checksum      )->by_default(5      )->as_number();
+    this->mm_per_arc_segment  = THEKERNEL->config->value(mm_per_arc_segment_checksum  )->by_default(    0.5f)->as_number();
+    this->arc_correction      = THEKERNEL->config->value(arc_correction_checksum      )->by_default(    5   )->as_number();
 
-    this->max_speeds[X_AXIS]  = THEKERNEL->config->value(x_axis_max_speed_checksum    )->by_default(60000  )->as_number();
-    this->max_speeds[Y_AXIS]  = THEKERNEL->config->value(y_axis_max_speed_checksum    )->by_default(60000  )->as_number();
-    this->max_speeds[Z_AXIS]  = THEKERNEL->config->value(z_axis_max_speed_checksum    )->by_default(300    )->as_number();
+    this->max_speeds[X_AXIS]  = THEKERNEL->config->value(x_axis_max_speed_checksum    )->by_default(60000.0F)->as_number() / 60.0F;
+    this->max_speeds[Y_AXIS]  = THEKERNEL->config->value(y_axis_max_speed_checksum    )->by_default(60000.0F)->as_number() / 60.0F;
+    this->max_speeds[Z_AXIS]  = THEKERNEL->config->value(z_axis_max_speed_checksum    )->by_default(  300.0F)->as_number() / 60.0F;
 
     Pin alpha_step_pin;
     Pin alpha_dir_pin;
@@ -180,10 +188,21 @@ void Robot::on_config_reload(void* argument){
     beta_stepper_motor->change_steps_per_mm(steps_per_mm[1]);
     gamma_stepper_motor->change_steps_per_mm(steps_per_mm[2]);
 
+    alpha_stepper_motor->max_rate = THEKERNEL->config->value(alpha_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
+    beta_stepper_motor->max_rate  = THEKERNEL->config->value(beta_max_rate_checksum )->by_default(30000.0F)->as_number() / 60.0F;
+    gamma_stepper_motor->max_rate = THEKERNEL->config->value(gamma_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
+
     actuators.clear();
     actuators.push_back(alpha_stepper_motor);
     actuators.push_back(beta_stepper_motor);
     actuators.push_back(gamma_stepper_motor);
+
+    // initialise actuator positions to current cartesian position (X0 Y0 Z0)
+    // so the first move can be correct if homing is not performed
+    float actuator_pos[3];
+    arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
+    for (int i = 0; i < 3; i++)
+        actuators[i]->change_last_milestone(actuator_pos[i]);
 }
 
 void Robot::on_get_public_data(void* argument){
@@ -193,15 +212,15 @@ void Robot::on_get_public_data(void* argument){
 
     if(pdr->second_element_is(speed_override_percent_checksum)) {
         static float return_data;
-        return_data= 100*this->seconds_per_minute/60;
+        return_data = 100.0F * 60.0F / seconds_per_minute;
         pdr->set_data_ptr(&return_data);
         pdr->set_taken();
 
     }else if(pdr->second_element_is(current_position_checksum)) {
         static float return_data[3];
-        return_data[0]= from_millimeters(this->current_position[0]);
-        return_data[1]= from_millimeters(this->current_position[1]);
-        return_data[2]= from_millimeters(this->current_position[2]);
+        return_data[0]= from_millimeters(this->last_milestone[0]);
+        return_data[1]= from_millimeters(this->last_milestone[1]);
+        return_data[2]= from_millimeters(this->last_milestone[2]);
 
         pdr->set_data_ptr(&return_data);
         pdr->set_taken();
@@ -217,9 +236,9 @@ void Robot::on_set_public_data(void* argument){
         // NOTE do not use this while printing!
         float t= *static_cast<float*>(pdr->get_data_ptr());
         // enforce minimum 10% speed
-        if (t < 10.0) t= 10.0;
+        if (t < 10.0F) t= 10.0F;
 
-        this->seconds_per_minute= t * 0.6;
+        this->seconds_per_minute = t / 0.6F; // t * 60 / 100
         pdr->set_taken();
     }
 }
@@ -256,11 +275,10 @@ void Robot::on_gcode_received(void * argument){
                             this->last_milestone[letter-'X'] = this->to_millimeters(gcode->get_value(letter));
                     }
                 }
-                memcpy(this->current_position, this->last_milestone, sizeof(float)*3); // current_position[] = last_milestone[];
 
                 // TODO: handle any number of actuators
                 float actuator_pos[3];
-                arm_solution->cartesian_to_actuator(current_position, actuator_pos);
+                arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
 
                 for (int i = 0; i < 3; i++)
                     actuators[i]->change_last_milestone(actuator_pos[i]);
@@ -286,22 +304,45 @@ void Robot::on_gcode_received(void * argument){
                 gcode->mark_as_taken();
                 return;
             case 114: gcode->stream->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
-                                                 from_millimeters(this->current_position[0]),
-                                                 from_millimeters(this->current_position[1]),
-                                                 from_millimeters(this->current_position[2]));
+                                                 from_millimeters(this->last_milestone[0]),
+                                                 from_millimeters(this->last_milestone[1]),
+                                                 from_millimeters(this->last_milestone[2]));
                 gcode->add_nl = true;
                 gcode->mark_as_taken();
                 return;
 
-            // TODO I'm not sure if the following is safe to do here, or should it go on the block queue?
+            case 203: // M203 Set maximum feedrates in mm/sec
+                if (gcode->has_letter('X'))
+                    this->max_speeds[X_AXIS]= gcode->get_value('X');
+                if (gcode->has_letter('Y'))
+                    this->max_speeds[Y_AXIS]= gcode->get_value('Y');
+                if (gcode->has_letter('Z'))
+                    this->max_speeds[Z_AXIS]= gcode->get_value('Z');
+                if (gcode->has_letter('A'))
+                    alpha_stepper_motor->max_rate= gcode->get_value('A');
+                if (gcode->has_letter('B'))
+                    beta_stepper_motor->max_rate= gcode->get_value('B');
+                if (gcode->has_letter('C'))
+                    gamma_stepper_motor->max_rate= gcode->get_value('C');
+
+                gcode->stream->printf("X:%g Y:%g Z:%g  A:%g B:%g C:%g ",
+                    this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
+                    alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate);
+                gcode->add_nl = true;
+                gcode->mark_as_taken();
+                break;
+
             case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
                 gcode->mark_as_taken();
+
                 if (gcode->has_letter('S'))
                 {
-                    float acc= gcode->get_value('S') * 60 * 60; // mm/min^2
+                    // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
+                    THEKERNEL->conveyor->wait_for_empty_queue();
+                    float acc= gcode->get_value('S'); // mm/s^2
                     // enforce minimum
-                    if (acc < 1.0)
-                        acc = 1.0;
+                    if (acc < 1.0F)
+                        acc = 1.0F;
                     THEKERNEL->planner->acceleration= acc;
                 }
                 break;
@@ -332,9 +373,13 @@ void Robot::on_gcode_received(void * argument){
                 {
                     float factor = gcode->get_value('S');
                     // enforce minimum 10% speed
-                    if (factor < 10.0)
-                        factor = 10.0;
-                    seconds_per_minute = factor * 0.6;
+                    if (factor < 10.0F)
+                        factor = 10.0F;
+                    // enforce maximum 10x speed
+                    if (factor > 1000.0F)
+                        factor = 1000.0F;
+
+                    seconds_per_minute = 6000.0F / factor;
                 }
                 break;
 
@@ -346,15 +391,19 @@ void Robot::on_gcode_received(void * argument){
             case 500: // M500 saves some volatile settings to config override file
             case 503: // M503 just prints the settings
                 gcode->stream->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm);
-                gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL->planner->acceleration/3600);
+                gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL->planner->acceleration);
                 gcode->stream->printf(";X- Junction Deviation, S - Minimum Planner speed:\nM205 X%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->minimum_planner_speed);
+                gcode->stream->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f A%1.5f B%1.5f C%1.5f\n",
+                    this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
+                    alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate);
                 gcode->mark_as_taken();
                 break;
 
-            case 665: // M665 set optional arm solution variables based on arm solution
+            case 665: // M665 set optional arm solution variables based on arm solution. NOTE these are not saved with M500
                 gcode->mark_as_taken();
-                // the parameter args could be any letter so try each one
+                // the parameter args could be any letter except S so try each one
                 for(char c='A';c<='Z';c++) {
+                    if(c == 'S') continue; // used for segments per second
                     float v;
                     bool supported= arm_solution->get_optional(c, &v); // retrieve current value if supported
 
@@ -367,8 +416,11 @@ void Robot::on_gcode_received(void * argument){
                         gcode->add_nl = true;
                     }
                 }
+                // set delta segments per second
+                if(gcode->has_letter('S')) {
+                    this->delta_segments_per_second= gcode->get_value('S');
+                }
                 break;
-
         }
     }
 
@@ -379,7 +431,7 @@ void Robot::on_gcode_received(void * argument){
     float target[3], offset[3];
     clear_vector(offset);
 
-    memcpy(target, this->current_position, sizeof(target));    //default to last target
+    memcpy(target, this->last_milestone, sizeof(target));    //default to last target
 
     for(char letter = 'I'; letter <= 'K'; letter++){
         if( gcode->has_letter(letter) ){
@@ -395,9 +447,9 @@ void Robot::on_gcode_received(void * argument){
     if( gcode->has_letter('F') )
     {
         if( this->motion_mode == MOTION_MODE_SEEK )
-            this->seek_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0F;
+            this->seek_rate = this->to_millimeters( gcode->get_value('F') );
         else
-            this->feed_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0F;
+            this->feed_rate = this->to_millimeters( gcode->get_value('F') );
     }
 
     //Perform any physical actions
@@ -405,8 +457,8 @@ void Robot::on_gcode_received(void * argument){
         case NEXT_ACTION_DEFAULT:
             switch(this->motion_mode){
                 case MOTION_MODE_CANCEL: break;
-                case MOTION_MODE_SEEK  : this->append_line(gcode, target, this->seek_rate ); break;
-                case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate ); break;
+                case MOTION_MODE_SEEK  : this->append_line(gcode, target, this->seek_rate / seconds_per_minute ); break;
+                case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate / seconds_per_minute ); break;
                 case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break;
             }
             break;
@@ -415,7 +467,7 @@ void Robot::on_gcode_received(void * argument){
     // As far as the parser is concerned, the position is now == target. In reality the
     // motion control system might still be processing the action and the real tool position
     // in any intermediate location.
-    memcpy(this->current_position, target, sizeof(this->current_position)); // this->position[] = target[];
+    memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->position[] = target[];
 
 }
 
@@ -430,36 +482,61 @@ void Robot::distance_in_gcode_is_known(Gcode* gcode){
 
 // Reset the position for all axes ( used in homing and G92 stuff )
 void Robot::reset_axis_position(float position, int axis) {
-    this->last_milestone[axis] = this->current_position[axis] = position;
-    actuators[axis]->change_last_milestone(position);
-}
+    this->last_milestone[axis] = position;
 
+    float actuator_pos[3];
+    arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
 
-// Convert target from millimeters to steps, and append this to the planner
-void Robot::append_milestone( float target[], float rate ){
-    float actuator_pos[3]; //Holds the result of the conversion
+    for (int i = 0; i < 3; i++)
+        actuators[i]->change_last_milestone(actuator_pos[i]);
+}
 
-    // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
-    this->arm_solution->cartesian_to_actuator( target, actuator_pos );
 
+// Convert target from millimeters to steps, and append this to the planner
+void Robot::append_milestone( float target[], float rate_mm_s )
+{
     float deltas[3];
-    for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
+    float unit_vec[3];
+    float actuator_pos[3];
+    float millimeters_of_travel;
+
+    // find distance moved by each axis
+    for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
+        deltas[axis] = target[axis] - last_milestone[axis];
 
     // Compute how long this move moves, so we can attach it to the block for later use
-    float millimeters_of_travel = sqrtf( pow( deltas[X_AXIS], 2 ) +  pow( deltas[Y_AXIS], 2 ) +  pow( deltas[Z_AXIS], 2 ) );
-
-    // Do not move faster than the configured limits
-    for(int axis=X_AXIS;axis<=Z_AXIS;axis++){
-        if( this->max_speeds[axis] > 0 ){
-            float axis_speed = ( fabs(deltas[axis]) / ( millimeters_of_travel / rate )) * seconds_per_minute;
-            if( axis_speed > this->max_speeds[axis] ){
-                rate = rate * ( this->max_speeds[axis] / axis_speed );
-            }
+    millimeters_of_travel = sqrtf( pow( deltas[X_AXIS], 2 ) +  pow( deltas[Y_AXIS], 2 ) +  pow( deltas[Z_AXIS], 2 ) );
+
+    // find distance unit vector
+    for (int i = 0; i < 3; i++)
+        unit_vec[i] = deltas[i] / millimeters_of_travel;
+
+    // Do not move faster than the configured cartesian limits
+    for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
+    {
+        if ( max_speeds[axis] > 0 )
+        {
+            float axis_speed = fabs(unit_vec[axis] * rate_mm_s);
+
+            if (axis_speed > max_speeds[axis])
+                rate_mm_s *= ( max_speeds[axis] / axis_speed );
         }
     }
 
+    // find actuator position given cartesian position
+    arm_solution->cartesian_to_actuator( target, actuator_pos );
+
+    // check per-actuator speed limits
+    for (int actuator = 0; actuator <= 2; actuator++)
+    {
+        float actuator_rate  = fabs(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * rate_mm_s / millimeters_of_travel;
+
+        if (actuator_rate > actuators[actuator]->max_rate)
+            rate_mm_s *= (actuators[actuator]->max_rate / actuator_rate);
+    }
+
     // Append the block to the planner
-    THEKERNEL->planner->append_block( actuator_pos, rate * seconds_per_minute, millimeters_of_travel, unit_vec );
+    THEKERNEL->planner->append_block( actuator_pos, rate_mm_s, millimeters_of_travel, unit_vec );
 
     // Update the last_milestone to the current target for the next time we use last_milestone
     memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->last_milestone[] = target[];
@@ -467,16 +544,18 @@ void Robot::append_milestone( float target[], float rate ){
 }
 
 // Append a move to the queue ( cutting it into segments if needed )
-void Robot::append_line(Gcode* gcode, float target[], float rate ){
+void Robot::append_line(Gcode* gcode, float target[], float rate_mm_s ){
 
     // Find out the distance for this gcode
-    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 ) );
+    gcode->millimeters_of_travel = pow( target[X_AXIS]-this->last_milestone[X_AXIS], 2 ) +  pow( target[Y_AXIS]-this->last_milestone[Y_AXIS], 2 ) +  pow( target[Z_AXIS]-this->last_milestone[Z_AXIS], 2 );
 
     // We ignore non-moves ( for example, extruder moves are not XYZ moves )
-    if( gcode->millimeters_of_travel < 0.0001F ){
+    if( gcode->millimeters_of_travel < 1e-8F ){
         return;
     }
 
+    gcode->millimeters_of_travel = sqrtf(gcode->millimeters_of_travel);
+
     // Mark the gcode as having a known distance
     this->distance_in_gcode_is_known( gcode );
 
@@ -490,7 +569,7 @@ void Robot::append_line(Gcode* gcode, float target[], float rate ){
         // segment based on current speed and requested segments per second
         // the faster the travel speed the fewer segments needed
         // NOTE rate is mm/sec and we take into account any speed override
-        float seconds = 60.0/seconds_per_minute * gcode->millimeters_of_travel / rate;
+        float seconds = gcode->millimeters_of_travel / rate_mm_s;
         segments= max(1, ceil(this->delta_segments_per_second * seconds));
         // TODO if we are only moving in Z on a delta we don't really need to segment at all
 
@@ -502,20 +581,30 @@ void Robot::append_line(Gcode* gcode, float target[], float rate ){
         }
     }
 
-    // A vector to keep track of the endpoint of each segment
-    float temp_target[3];
-    //Initialize axes
-    memcpy( temp_target, this->current_position, sizeof(temp_target)); // temp_target[] = this->current_position[];
-
-    //For each segment
-    for( int i=0; i<segments-1; i++ ){
-        for(int axis=X_AXIS; axis <= Z_AXIS; axis++ ){ temp_target[axis] += ( target[axis]-this->current_position[axis] )/segments; }
-        // Append the end of this segment to the queue
-        this->append_milestone(temp_target, rate);
+    if (segments > 1)
+    {
+        // A vector to keep track of the endpoint of each segment
+        float segment_delta[3];
+        float segment_end[3];
+
+        // How far do we move each segment?
+        for (int i = X_AXIS; i <= Z_AXIS; i++)
+            segment_delta[i] = (target[i] - last_milestone[i]) / segments;
+
+        // segment 0 is already done - it's the end point of the previous move so we start at segment 1
+        // We always add another point after this loop so we stop at segments-1, ie i < segments
+        for (int i = 1; i < segments; i++)
+        {
+            for(int axis=X_AXIS; axis <= Z_AXIS; axis++ )
+                segment_end[axis] = last_milestone[axis] + segment_delta[axis];
+
+            // Append the end of this segment to the queue
+            this->append_milestone(segment_end, rate_mm_s);
+        }
     }
 
     // Append the end of this full move to the queue
-    this->append_milestone(target, rate);
+    this->append_milestone(target, rate_mm_s);
 
     // if adding these blocks didn't start executing, do that now
     THEKERNEL->conveyor->ensure_running();
@@ -526,9 +615,9 @@ void Robot::append_line(Gcode* gcode, float target[], float rate ){
 void Robot::append_arc(Gcode* gcode, float target[], float offset[], float radius, bool is_clockwise ){
 
     // Scary math
-    float center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
-    float center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
-    float linear_travel = target[this->plane_axis_2] - this->current_position[this->plane_axis_2];
+    float center_axis0 = this->last_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
+    float center_axis1 = this->last_milestone[this->plane_axis_1] + offset[this->plane_axis_1];
+    float linear_travel = target[this->plane_axis_2] - this->last_milestone[this->plane_axis_2];
     float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
     float r_axis1 = -offset[this->plane_axis_1];
     float rt_axis0 = target[this->plane_axis_0] - center_axis0;
@@ -589,7 +678,7 @@ void Robot::append_arc(Gcode* gcode, float target[], float offset[], float radiu
     int8_t count = 0;
 
     // Initialize the linear axis
-    arc_target[this->plane_axis_2] = this->current_position[this->plane_axis_2];
+    arc_target[this->plane_axis_2] = this->last_milestone[this->plane_axis_2];
 
     for (i = 1; i<segments; i++) { // Increment (segments-1)
 
@@ -615,12 +704,12 @@ void Robot::append_arc(Gcode* gcode, float target[], float offset[], float radiu
         arc_target[this->plane_axis_2] += linear_per_segment;
 
         // Append this segment to the queue
-        this->append_milestone(arc_target, this->feed_rate);
+        this->append_milestone(arc_target, this->feed_rate / seconds_per_minute);
 
     }
 
     // Ensure last segment arrives at target location.
-    this->append_milestone(target, this->feed_rate);
+    this->append_milestone(target, this->feed_rate / seconds_per_minute);
 }
 
 // Do the math for an arc and add it to the queue