From 3b931be8cf608f0ada9fc9d49ce1e0fed290da10 Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Thu, 20 Sep 2018 20:25:38 -0400
Subject: [PATCH] move_to_pos uses setpoints instead of estimates.  Add
 A_to_cpss

---
 Firmware/MotorControl/controller.cpp | 12 +++++-------
 Firmware/MotorControl/controller.hpp |  2 +-
 Firmware/MotorControl/trapTraj.hpp   |  1 +
 3 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 7aae7fb2..d3f9870c 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -44,16 +44,15 @@ void Controller::set_current_setpoint(float current_setpoint) {
 #endif
 }
 
-void Controller::move_to_pos(float pos_setpoint) {
-    planned_move_end_time_ = axis_->trap_.planTrapezoidal(pos_setpoint, axis_->encoder_.pos_estimate_,
-                                                          axis_->encoder_.vel_estimate_, axis_->trap_.config_.vel_limit,
+void Controller::move_to_pos(float goal_point) {
+    planned_move_end_time_ = axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_,
+                                                          vel_setpoint_, axis_->trap_.config_.vel_limit,
                                                           axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit);
     config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL;
     TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f);
     pos_setpoint_ = myTraj.Y;
     vel_setpoint_ = myTraj.Yd;
-    // current_setpoint_ = myTraj.Ydd;
-    current_setpoint_ = 0.0f; // Temporary, until we have a way to convert from accel to current
+    current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.A_to_cpss;
 
     planned_move_timer_ = axis_->loop_counter_ * current_meas_period;
 }
@@ -109,8 +108,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
             TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_);
             pos_setpoint_ = myTraj.Y;
             vel_setpoint_ = myTraj.Yd;
-            // current_setpoint_ = myTraj.Ydd;
-            current_setpoint_ = 0.0f; // Temporary, until we have a way of converting from accel to current
+            current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.A_to_cpss;
         }
         anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
     }
diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
index b4e904b6..dd3f76a5 100644
--- a/Firmware/MotorControl/controller.hpp
+++ b/Firmware/MotorControl/controller.hpp
@@ -34,7 +34,7 @@ public:
     void set_current_setpoint(float current_setpoint);
 
     // Trajectory-Planned control
-    void move_to_pos(float pos_setpoint);
+    void move_to_pos(float goal_point);
     
     // TODO: make this more similar to other calibration loops
     void start_anticogging_calibration();
diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp
index 60001218..cdfe5bc4 100644
--- a/Firmware/MotorControl/trapTraj.hpp
+++ b/Firmware/MotorControl/trapTraj.hpp
@@ -5,6 +5,7 @@ struct TrapTrajConfig_t {
     float vel_limit = 20000.0f;
     float accel_limit = 5000.0f;
     float decel_limit = 5000.0f;
+    float A_to_cpss = 0.0f;
 };
 
 struct TrapTrajStep_t {