Fix odd behaviour during homing

Because the accelerations and velocities are so low during this phase, we were overshooting quite a bit.  Also, the pos_setpoint was wrong during the calculation of the trap_traj, so it was all sorts of screwed up.

Firmware/MotorControl/axis.cpp

@@ -317,6 +317,11 @@ bool Axis::run_closed_loop_control_loop() {
         // Handle the homing case
         if (homing_.homing_state == HOMING_STATE_HOMING) {
             if (min_endstop_.getEndstopState()) {
+                // pos_setpoint is the starting position for the trap_traj so we need to set it.
+                controller_.pos_setpoint_ = min_endstop_.config_.offset;
+                controller_.vel_setpoint_ = 0.0f;  // Change directions without decelerating
+
+                // Set our current position in encoder counts to make control more logical
                 encoder_.set_linear_count(min_endstop_.config_.offset);
 
                 controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;