y2014/control_loops/drivetrain/drivetrain.cc - RealtimeRoboticsGroup/test - Gitiles

 #include "y2014/control_loops/drivetrain/drivetrain.h"

 #include <stdio.h>
 #include <sched.h>
 #include <cmath>
 #include <memory>
 #include "Eigen/Dense"

 #include "aos/common/logging/logging.h"
 #include "aos/common/controls/polytope.h"
 #include "aos/common/commonmath.h"
 #include "aos/common/logging/queue_logging.h"
 #include "aos/common/logging/matrix_logging.h"

 #include "y2014/constants.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/coerce_goal.h"
 #include "y2014/control_loops/drivetrain/drivetrain.q.h"
 #include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
 #include "y2014/control_loops/drivetrain/polydrivetrain.h"
 #include "frc971/queues/gyro.q.h"
 #include "frc971/shifter_hall_effect.h"

 // A consistent way to mark code that goes away without shifters. It's still
 // here because we will have shifters again in the future.
 #define HAVE_SHIFTERS 1

 using frc971::sensors::gyro_reading;

 namespace frc971 {
 namespace control_loops {

 using ::y2014::control_loops::drivetrain::kDt;

 class DrivetrainMotorsSS {
  public:
   class LimitedDrivetrainLoop : public StateFeedbackLoop<4, 2, 2> {
    public:
     LimitedDrivetrainLoop(StateFeedbackLoop<4, 2, 2> &&loop)
         : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
         U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
                  -1, 0,
                  0, 1,
                  0, -1).finished(),
                 (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0,
                  12.0, 12.0).finished()) {
       ::aos::controls::HPolytope<0>::Init();
       T << 1, -1, 1, 1;
       T_inverse = T.inverse();
     }

     bool output_was_capped() const {
       return output_was_capped_;
     }

    private:
     virtual void CapU() {
       const Eigen::Matrix<double, 4, 1> error = R() - X_hat();

       if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
         mutable_U() =
             U() * 12.0 / ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
         LOG_MATRIX(DEBUG, "U is now", U());
         // TODO(Austin): Figure out why the polytope stuff wasn't working and
         // remove this hack.
         output_was_capped_ = true;
         return;

         LOG_MATRIX(DEBUG, "U at start", U());
         LOG_MATRIX(DEBUG, "R at start", R());
         LOG_MATRIX(DEBUG, "Xhat at start", X_hat());

         Eigen::Matrix<double, 2, 2> position_K;
         position_K << K(0, 0), K(0, 2),
                    K(1, 0), K(1, 2);
         Eigen::Matrix<double, 2, 2> velocity_K;
         velocity_K << K(0, 1), K(0, 3),
                    K(1, 1), K(1, 3);

         Eigen::Matrix<double, 2, 1> position_error;
         position_error << error(0, 0), error(2, 0);
         const auto drive_error = T_inverse * position_error;
         Eigen::Matrix<double, 2, 1> velocity_error;
         velocity_error << error(1, 0), error(3, 0);
         LOG_MATRIX(DEBUG, "error", error);

         const auto &poly = U_Poly_;
         const Eigen::Matrix<double, 4, 2> pos_poly_H =
             poly.H() * position_K * T;
         const Eigen::Matrix<double, 4, 1> pos_poly_k =
             poly.k() - poly.H() * velocity_K * velocity_error;
         const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);

         Eigen::Matrix<double, 2, 1> adjusted_pos_error;
         {
           const auto &P = drive_error;

           Eigen::Matrix<double, 1, 2> L45;
           L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
           const double w45 = 0;

           Eigen::Matrix<double, 1, 2> LH;
           if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
             LH << 0, 1;
           } else {
             LH << 1, 0;
           }
           const double wh = LH.dot(P);

           Eigen::Matrix<double, 2, 2> standard;
           standard << L45, LH;
           Eigen::Matrix<double, 2, 1> W;
           W << w45, wh;
           const Eigen::Matrix<double, 2, 1> intersection =
               standard.inverse() * W;

           bool is_inside_h;
           const auto adjusted_pos_error_h =
               DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
           const auto adjusted_pos_error_45 =
               DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
           if (pos_poly.IsInside(intersection)) {
             adjusted_pos_error = adjusted_pos_error_h;
           } else {
             if (is_inside_h) {
               if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
                 adjusted_pos_error = adjusted_pos_error_h;
               } else {
                 adjusted_pos_error = adjusted_pos_error_45;
               }
             } else {
               adjusted_pos_error = adjusted_pos_error_45;
             }
           }
         }

         LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
         mutable_U() =
             velocity_K * velocity_error + position_K * T * adjusted_pos_error;
         LOG_MATRIX(DEBUG, "U is now", U());
       } else {
         output_was_capped_ = false;
       }
     }

     const ::aos::controls::HPolytope<2> U_Poly_;
     Eigen::Matrix<double, 2, 2> T, T_inverse;
     bool output_was_capped_ = false;;
   };

   DrivetrainMotorsSS()
       : loop_(new LimitedDrivetrainLoop(
             constants::GetValues().make_drivetrain_loop())),
         filtered_offset_(0.0),
         gyro_(0.0),
         left_goal_(0.0),
         right_goal_(0.0),
         raw_left_(0.0),
         raw_right_(0.0) {
     // High gear on both.
     loop_->set_controller_index(3);
   }

   void SetGoal(double left, double left_velocity, double right,
                double right_velocity) {
     left_goal_ = left;
     right_goal_ = right;
     loop_->mutable_R() << left, left_velocity, right, right_velocity;
   }
   void SetRawPosition(double left, double right) {
     raw_right_ = right;
     raw_left_ = left;
     Eigen::Matrix<double, 2, 1> Y;
     Y << left + filtered_offset_, right - filtered_offset_;
     loop_->Correct(Y);
   }
   void SetPosition(double left, double right, double gyro) {
     // Decay the offset quickly because this gyro is great.
     const double offset =
         (right - left - gyro * constants::GetValues().turn_width) / 2.0;
     filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
     gyro_ = gyro;
     SetRawPosition(left, right);
   }

   void SetExternalMotors(double left_voltage, double right_voltage) {
     loop_->mutable_U() << left_voltage, right_voltage;
   }

   void Update(bool stop_motors, bool enable_control_loop) {
     if (enable_control_loop) {
       loop_->Update(stop_motors);
     } else {
       if (stop_motors) {
         loop_->mutable_U().setZero();
         loop_->mutable_U_uncapped().setZero();
       }
       loop_->UpdateObserver();
     }
     ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
     LOG_MATRIX(DEBUG, "E", E);
   }

   double GetEstimatedRobotSpeed() const {
     // lets just call the average of left and right velocities close enough
     return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
   }

   double GetEstimatedLeftEncoder() const {
     return loop_->X_hat(0, 0);
   }

   double GetEstimatedRightEncoder() const {
     return loop_->X_hat(2, 0);
   }

   bool OutputWasCapped() const {
     return loop_->output_was_capped();
   }

   void SendMotors(DrivetrainQueue::Output *output) const {
     if (output) {
       output->left_voltage = loop_->U(0, 0);
       output->right_voltage = loop_->U(1, 0);
       output->left_high = true;
       output->right_high = true;
     }
   }

   const LimitedDrivetrainLoop &loop() const { return *loop_; }

  private:
   ::std::unique_ptr<LimitedDrivetrainLoop> loop_;

   double filtered_offset_;
   double gyro_;
   double left_goal_;
   double right_goal_;
   double raw_left_;
   double raw_right_;
 };


 void DrivetrainLoop::RunIteration(const DrivetrainQueue::Goal *goal,
                                   const DrivetrainQueue::Position *position,
                                   DrivetrainQueue::Output *output,
                                   DrivetrainQueue::Status * status) {
   // TODO(aschuh): These should be members of the class.
   static DrivetrainMotorsSS dt_closedloop;
   static PolyDrivetrain dt_openloop;

   bool bad_pos = false;
   if (position == nullptr) {
     LOG_INTERVAL(no_position_);
     bad_pos = true;
   }
   no_position_.Print();

   bool control_loop_driving = false;
   if (goal) {
     double wheel = goal->steering;
     double throttle = goal->throttle;
     bool quickturn = goal->quickturn;
 #if HAVE_SHIFTERS
     bool highgear = goal->highgear;
 #endif

     control_loop_driving = goal->control_loop_driving;
     double left_goal = goal->left_goal;
     double right_goal = goal->right_goal;

     dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
                           goal->right_velocity_goal);
 #if HAVE_SHIFTERS
     dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
 #else
     dt_openloop.SetGoal(wheel, throttle, quickturn, false);
 #endif
   }

   if (!bad_pos) {
     const double left_encoder = position->left_encoder;
     const double right_encoder = position->right_encoder;
     if (gyro_reading.FetchLatest()) {
       LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
       dt_closedloop.SetPosition(left_encoder, right_encoder,
                                 gyro_reading->angle);
     } else {
       dt_closedloop.SetRawPosition(left_encoder, right_encoder);
     }
   }
   dt_openloop.SetPosition(position);
   dt_openloop.Update();

   if (control_loop_driving) {
     dt_closedloop.Update(output == NULL, true);
     dt_closedloop.SendMotors(output);
   } else {
     dt_openloop.SendMotors(output);
     if (output) {
       dt_closedloop.SetExternalMotors(output->left_voltage,
                                       output->right_voltage);
     }
     dt_closedloop.Update(output == NULL, false);
   }

   // set the output status of the control loop state
   if (status) {
     bool done = false;
     if (goal) {
       done = ((::std::abs(goal->left_goal -
                           dt_closedloop.GetEstimatedLeftEncoder()) <
                constants::GetValues().drivetrain_done_distance) &&
               (::std::abs(goal->right_goal -
                           dt_closedloop.GetEstimatedRightEncoder()) <
                constants::GetValues().drivetrain_done_distance));
     }
     status->is_done = done;
     status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
     status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
     status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();

     status->filtered_left_velocity = dt_closedloop.loop().X_hat(1, 0);
     status->filtered_right_velocity = dt_closedloop.loop().X_hat(3, 0);
     status->output_was_capped = dt_closedloop.OutputWasCapped();
     status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
     status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
   }
 }

 }  // namespace control_loops
 }  // namespace frc971
	#include "y2014/control_loops/drivetrain/drivetrain.h"

	#include <stdio.h>
	#include <sched.h>
	#include <cmath>
	#include <memory>
	#include "Eigen/Dense"

	#include "aos/common/logging/logging.h"
	#include "aos/common/controls/polytope.h"
	#include "aos/common/commonmath.h"
	#include "aos/common/logging/queue_logging.h"
	#include "aos/common/logging/matrix_logging.h"

	#include "y2014/constants.h"
	#include "frc971/control_loops/state_feedback_loop.h"
	#include "frc971/control_loops/coerce_goal.h"
	#include "y2014/control_loops/drivetrain/drivetrain.q.h"
	#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
	#include "y2014/control_loops/drivetrain/polydrivetrain.h"
	#include "frc971/queues/gyro.q.h"
	#include "frc971/shifter_hall_effect.h"

	// A consistent way to mark code that goes away without shifters. It's still
	// here because we will have shifters again in the future.
	#define HAVE_SHIFTERS 1

	using frc971::sensors::gyro_reading;

	namespace frc971 {
	namespace control_loops {

	using ::y2014::control_loops::drivetrain::kDt;

	class DrivetrainMotorsSS {
	public:
	class LimitedDrivetrainLoop : public StateFeedbackLoop<4, 2, 2> {
	public:
	LimitedDrivetrainLoop(StateFeedbackLoop<4, 2, 2> &&loop)
	: StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
	U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
	-1, 0,
	0, 1,
	0, -1).finished(),
	(Eigen::Matrix<double, 4, 1>() << 12.0, 12.0,
	12.0, 12.0).finished()) {
	::aos::controls::HPolytope<0>::Init();
	T << 1, -1, 1, 1;
	T_inverse = T.inverse();
	}

	bool output_was_capped() const {
	return output_was_capped_;
	}

	private:
	virtual void CapU() {
	const Eigen::Matrix<double, 4, 1> error = R() - X_hat();

	if (::std::abs(U(0, 0)) > 12.0 \|\| ::std::abs(U(1, 0)) > 12.0) {
	mutable_U() =
	U() * 12.0 / ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
	LOG_MATRIX(DEBUG, "U is now", U());
	// TODO(Austin): Figure out why the polytope stuff wasn't working and
	// remove this hack.
	output_was_capped_ = true;
	return;

	LOG_MATRIX(DEBUG, "U at start", U());
	LOG_MATRIX(DEBUG, "R at start", R());
	LOG_MATRIX(DEBUG, "Xhat at start", X_hat());

	Eigen::Matrix<double, 2, 2> position_K;
	position_K << K(0, 0), K(0, 2),
	K(1, 0), K(1, 2);
	Eigen::Matrix<double, 2, 2> velocity_K;
	velocity_K << K(0, 1), K(0, 3),
	K(1, 1), K(1, 3);

	Eigen::Matrix<double, 2, 1> position_error;
	position_error << error(0, 0), error(2, 0);
	const auto drive_error = T_inverse * position_error;
	Eigen::Matrix<double, 2, 1> velocity_error;
	velocity_error << error(1, 0), error(3, 0);
	LOG_MATRIX(DEBUG, "error", error);

	const auto &poly = U_Poly_;
	const Eigen::Matrix<double, 4, 2> pos_poly_H =
	poly.H() * position_K * T;
	const Eigen::Matrix<double, 4, 1> pos_poly_k =
	poly.k() - poly.H() * velocity_K * velocity_error;
	const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);

	Eigen::Matrix<double, 2, 1> adjusted_pos_error;
	{
	const auto &P = drive_error;

	Eigen::Matrix<double, 1, 2> L45;
	L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
	const double w45 = 0;

	Eigen::Matrix<double, 1, 2> LH;
	if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
	LH << 0, 1;
	} else {
	LH << 1, 0;
	}
	const double wh = LH.dot(P);

	Eigen::Matrix<double, 2, 2> standard;
	standard << L45, LH;
	Eigen::Matrix<double, 2, 1> W;
	W << w45, wh;
	const Eigen::Matrix<double, 2, 1> intersection =
	standard.inverse() * W;

	bool is_inside_h;
	const auto adjusted_pos_error_h =
	DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
	const auto adjusted_pos_error_45 =
	DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
	if (pos_poly.IsInside(intersection)) {
	adjusted_pos_error = adjusted_pos_error_h;
	} else {
	if (is_inside_h) {
	if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
	adjusted_pos_error = adjusted_pos_error_h;
	} else {
	adjusted_pos_error = adjusted_pos_error_45;
	}
	} else {
	adjusted_pos_error = adjusted_pos_error_45;
	}
	}
	}

	LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
	mutable_U() =
	velocity_K * velocity_error + position_K * T * adjusted_pos_error;
	LOG_MATRIX(DEBUG, "U is now", U());
	} else {
	output_was_capped_ = false;
	}
	}

	const ::aos::controls::HPolytope<2> U_Poly_;
	Eigen::Matrix<double, 2, 2> T, T_inverse;
	bool output_was_capped_ = false;;
	};

	DrivetrainMotorsSS()
	: loop_(new LimitedDrivetrainLoop(
	constants::GetValues().make_drivetrain_loop())),
	filtered_offset_(0.0),
	gyro_(0.0),
	left_goal_(0.0),
	right_goal_(0.0),
	raw_left_(0.0),
	raw_right_(0.0) {
	// High gear on both.
	loop_->set_controller_index(3);
	}

	void SetGoal(double left, double left_velocity, double right,
	double right_velocity) {
	left_goal_ = left;
	right_goal_ = right;
	loop_->mutable_R() << left, left_velocity, right, right_velocity;
	}
	void SetRawPosition(double left, double right) {
	raw_right_ = right;
	raw_left_ = left;
	Eigen::Matrix<double, 2, 1> Y;
	Y << left + filtered_offset_, right - filtered_offset_;
	loop_->Correct(Y);
	}
	void SetPosition(double left, double right, double gyro) {
	// Decay the offset quickly because this gyro is great.
	const double offset =
	(right - left - gyro * constants::GetValues().turn_width) / 2.0;
	filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
	gyro_ = gyro;
	SetRawPosition(left, right);
	}

	void SetExternalMotors(double left_voltage, double right_voltage) {
	loop_->mutable_U() << left_voltage, right_voltage;
	}

	void Update(bool stop_motors, bool enable_control_loop) {
	if (enable_control_loop) {
	loop_->Update(stop_motors);
	} else {
	if (stop_motors) {
	loop_->mutable_U().setZero();
	loop_->mutable_U_uncapped().setZero();
	}
	loop_->UpdateObserver();
	}
	::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
	LOG_MATRIX(DEBUG, "E", E);
	}

	double GetEstimatedRobotSpeed() const {
	// lets just call the average of left and right velocities close enough
	return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
	}

	double GetEstimatedLeftEncoder() const {
	return loop_->X_hat(0, 0);
	}

	double GetEstimatedRightEncoder() const {
	return loop_->X_hat(2, 0);
	}

	bool OutputWasCapped() const {
	return loop_->output_was_capped();
	}

	void SendMotors(DrivetrainQueue::Output *output) const {
	if (output) {
	output->left_voltage = loop_->U(0, 0);
	output->right_voltage = loop_->U(1, 0);
	output->left_high = true;
	output->right_high = true;
	}
	}

	const LimitedDrivetrainLoop &loop() const { return *loop_; }

	private:
	::std::unique_ptr<LimitedDrivetrainLoop> loop_;

	double filtered_offset_;
	double gyro_;
	double left_goal_;
	double right_goal_;
	double raw_left_;
	double raw_right_;
	};


	void DrivetrainLoop::RunIteration(const DrivetrainQueue::Goal *goal,
	const DrivetrainQueue::Position *position,
	DrivetrainQueue::Output *output,
	DrivetrainQueue::Status * status) {
	// TODO(aschuh): These should be members of the class.
	static DrivetrainMotorsSS dt_closedloop;
	static PolyDrivetrain dt_openloop;

	bool bad_pos = false;
	if (position == nullptr) {
	LOG_INTERVAL(no_position_);
	bad_pos = true;
	}
	no_position_.Print();

	bool control_loop_driving = false;
	if (goal) {
	double wheel = goal->steering;
	double throttle = goal->throttle;
	bool quickturn = goal->quickturn;
	#if HAVE_SHIFTERS
	bool highgear = goal->highgear;
	#endif

	control_loop_driving = goal->control_loop_driving;
	double left_goal = goal->left_goal;
	double right_goal = goal->right_goal;

	dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
	goal->right_velocity_goal);
	#if HAVE_SHIFTERS
	dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
	#else
	dt_openloop.SetGoal(wheel, throttle, quickturn, false);
	#endif
	}

	if (!bad_pos) {
	const double left_encoder = position->left_encoder;
	const double right_encoder = position->right_encoder;
	if (gyro_reading.FetchLatest()) {
	LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
	dt_closedloop.SetPosition(left_encoder, right_encoder,
	gyro_reading->angle);
	} else {
	dt_closedloop.SetRawPosition(left_encoder, right_encoder);
	}
	}
	dt_openloop.SetPosition(position);
	dt_openloop.Update();

	if (control_loop_driving) {
	dt_closedloop.Update(output == NULL, true);
	dt_closedloop.SendMotors(output);
	} else {
	dt_openloop.SendMotors(output);
	if (output) {
	dt_closedloop.SetExternalMotors(output->left_voltage,
	output->right_voltage);
	}
	dt_closedloop.Update(output == NULL, false);
	}

	// set the output status of the control loop state
	if (status) {
	bool done = false;
	if (goal) {
	done = ((::std::abs(goal->left_goal -
	dt_closedloop.GetEstimatedLeftEncoder()) <
	constants::GetValues().drivetrain_done_distance) &&
	(::std::abs(goal->right_goal -
	dt_closedloop.GetEstimatedRightEncoder()) <
	constants::GetValues().drivetrain_done_distance));
	}
	status->is_done = done;
	status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
	status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
	status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();

	status->filtered_left_velocity = dt_closedloop.loop().X_hat(1, 0);
	status->filtered_right_velocity = dt_closedloop.loop().X_hat(3, 0);
	status->output_was_capped = dt_closedloop.OutputWasCapped();
	status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
	status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
	}
	}

	} // namespace control_loops
	} // namespace frc971