Port y2014 bot3 to new control system.
Change-Id: I894277089335e36ea95b52e033056b1a8fb4ca30
diff --git a/y2014_bot3/control_loops/drivetrain/BUILD b/y2014_bot3/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..c1b1433
--- /dev/null
+++ b/y2014_bot3/control_loops/drivetrain/BUILD
@@ -0,0 +1,95 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+cc_binary(
+ name = 'replay_drivetrain_y2014_bot3',
+ srcs = [
+ 'replay_drivetrain.cc',
+ ],
+ deps = [
+ ':drivetrain_queue',
+ '//aos/common/controls:replay_control_loop',
+ '//aos/linux_code:init',
+ ],
+)
+
+queue_library(
+ name = 'drivetrain_queue',
+ srcs = [
+ 'drivetrain.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ ],
+)
+
+cc_library(
+ name = 'polydrivetrain_plants',
+ srcs = [
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'drivetrain_dog_motor_plant.cc',
+ 'polydrivetrain_cim_plant.cc',
+ ],
+ hdrs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.h',
+ 'polydrivetrain_cim_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'drivetrain_lib',
+ hdrs = [
+ 'drivetrain.h',
+ ],
+ srcs = [
+ 'drivetrain.cc',
+ ],
+ deps = [
+ ':polydrivetrain_plants',
+ ':drivetrain_queue',
+ '//aos/common/controls:control_loop',
+ '//aos/common/controls:polytope',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:coerce_goal',
+ '//frc971/queues:gyro',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/logging:matrix_logging',
+ '//aos/common:math',
+ '//aos/common/util:log_interval',
+ '//frc971:shifter_hall_effect',
+ ],
+)
+
+cc_test(
+ name = 'drivetrain_lib_test',
+ srcs = [
+ 'drivetrain_lib_test.cc',
+ ],
+ deps = [
+ '//aos/testing:googletest',
+ ':drivetrain_queue',
+ ':drivetrain_lib',
+ '//aos/common/controls:control_loop_test',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/queues:gyro',
+ '//aos/common:queues',
+ '//aos/common/network:team_number',
+ ],
+)
+
+cc_binary(
+ name = 'drivetrain_y2014_bot3',
+ srcs = [
+ 'drivetrain_main.cc',
+ ],
+ deps = [
+ ':drivetrain_lib',
+ ':drivetrain_queue',
+ '//aos/linux_code:init',
+ ],
+)
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.cc b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
index 3a40836..047cb71 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
@@ -1,12 +1,9 @@
-#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
-#include <math.h>
#include <stdio.h>
#include <sched.h>
-
-#include <functional>
+#include <cmath>
#include <memory>
-
#include "Eigen/Dense"
#include "aos/common/logging/logging.h"
@@ -15,21 +12,18 @@
#include "aos/common/logging/queue_logging.h"
#include "aos/common/logging/matrix_logging.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
-#include "bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "bot3/shifter_hall_effect.h"
-#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "frc971/control_loops/coerce_goal.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/queues/gyro.q.h"
+#include "frc971/shifter_hall_effect.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::sensors::gyro_reading;
-using ::frc971::control_loops::DoCoerceGoal;
-using ::frc971::control_loops::CoerceGoal;
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
class DrivetrainMotorsSS {
@@ -38,35 +32,38 @@
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()) {
+ 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_;
- }
+ 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);
+ 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);
+ 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);
@@ -106,10 +103,11 @@
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_h = frc971::control_loops::DoCoerceGoal(
+ pos_poly, LH, wh, drive_error, &is_inside_h);
const auto adjusted_pos_error_45 =
- DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+ frc971::control_loops::DoCoerceGoal(pos_poly, L45, w45,
+ intersection, nullptr);
if (pos_poly.IsInside(intersection)) {
adjusted_pos_error = adjusted_pos_error_h;
} else {
@@ -136,12 +134,12 @@
const ::aos::controls::HPolytope<2> U_Poly_;
Eigen::Matrix<double, 2, 2> T, T_inverse;
- bool output_was_capped_ = false;;
+ bool output_was_capped_ = false;
};
DrivetrainMotorsSS()
: loop_(new LimitedDrivetrainLoop(
- MakeDrivetrainLoop())),
+ ::y2014_bot3::control_loops::MakeDrivetrainLoop())),
filtered_offset_(0.0),
gyro_(0.0),
left_goal_(0.0),
@@ -167,9 +165,7 @@
}
void SetPosition(double left, double right, double gyro) {
// Decay the offset quickly because this gyro is great.
- const double offset =
- (right - left - gyro * kBot3TurnWidth) / 2.0;
- // TODO(brians): filtered_offset_ = offset first time around.
+ const double offset = (right - left - gyro * kDrivetrainTurnWidth) / 2.0;
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
SetRawPosition(left, right);
@@ -198,19 +194,13 @@
return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
}
- double GetEstimatedLeftEncoder() const {
- return loop_->X_hat(0, 0);
- }
+ double GetEstimatedLeftEncoder() const { return loop_->X_hat(0, 0); }
- double GetEstimatedRightEncoder() const {
- return loop_->X_hat(2, 0);
- }
+ double GetEstimatedRightEncoder() const { return loop_->X_hat(2, 0); }
- bool OutputWasCapped() const {
- return loop_->output_was_capped();
- }
+ bool OutputWasCapped() const { return loop_->output_was_capped(); }
- void SendMotors(Drivetrain::Output *output) const {
+ void SendMotors(DrivetrainQueue::Output *output) const {
if (output) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
@@ -234,31 +224,26 @@
class PolyDrivetrain {
public:
-
- enum Gear {
- HIGH,
- LOW,
- SHIFTING_UP,
- SHIFTING_DOWN
- };
+ enum Gear { HIGH, LOW };
// Stall Torque in N m
static constexpr double kStallTorque = 2.42;
// Stall Current in Amps
- static constexpr double kStallCurrent = 133;
+ static constexpr double kStallCurrent = 133.0;
// Free Speed in RPM. Used number from last year.
static constexpr double kFreeSpeed = 4650.0;
// Free Current in Amps
static constexpr double kFreeCurrent = 2.7;
// Moment of inertia of the drivetrain in kg m^2
// Just borrowed from last year.
- static constexpr double J = 6.4;
+ static constexpr double J = 10;
// Mass of the robot, in kg.
static constexpr double m = 68;
// Radius of the robot, in meters (from last year).
- static constexpr double rb = 0.617998644 / 2.0;
+ static constexpr double rb = 0.66675 / 2.0;
static constexpr double kWheelRadius = 0.04445;
// Resistance of the motor, divided by the number of motors.
- static constexpr double kR = (12.0 / kStallCurrent / 4 + 0.03) / (0.93 * 0.93);
+ static constexpr double kR =
+ (12.0 / kStallCurrent / 4 + 0.03) / (0.93 * 0.93);
// Motor velocity constant
static constexpr double Kv =
((kFreeSpeed / 60.0 * 2.0 * M_PI) / (12.0 - kR * kFreeCurrent));
@@ -275,7 +260,7 @@
/*[*/ 12 /*]*/,
/*[*/ 12 /*]]*/).finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(
- MakeVelocityDrivetrainLoop())),
+ ::y2014_bot3::control_loops::MakeVelocityDrivetrainLoop())),
ttrust_(1.1),
wheel_(0.0),
throttle_(0.0),
@@ -285,84 +270,44 @@
left_gear_(LOW),
right_gear_(LOW),
counter_(0) {
-
last_position_.Zero();
position_.Zero();
}
- static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
- static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
- double shifter_position, double velocity, Gear gear) {
- // TODO(austin): G_high, G_low and kWheelRadius
- const double avg_hall_effect =
- (hall_effect.clear_high + hall_effect.clear_low) / 2.0;
-
- const bool use_high =
- kBot3SimpleShifting ? gear == HIGH : shifter_position > avg_hall_effect;
- if (use_high) {
- return velocity / kBot3HighGearRatio / kWheelRadius;
+ static double MotorSpeed(bool high_gear, double velocity) {
+ if (high_gear) {
+ return velocity / kDrivetrainHighGearRatio / kWheelRadius;
} else {
- return velocity / kBot3LowGearRatio / kWheelRadius;
+ return velocity / kDrivetrainLowGearRatio / kWheelRadius;
}
}
void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
- const double kWheelNonLinearity = 0.3;
+ const double kWheelNonLinearity = 0.5;
// Apply a sin function that's scaled to make it feel better.
const double angular_range = M_PI_2 * kWheelNonLinearity;
+
+ quickturn_ = quickturn;
wheel_ = sin(angular_range * wheel) / sin(angular_range);
wheel_ = sin(angular_range * wheel_) / sin(angular_range);
- quickturn_ = quickturn;
+ if (!quickturn_) {
+ wheel_ *= 1.5;
+ }
static const double kThrottleDeadband = 0.05;
if (::std::abs(throttle) < kThrottleDeadband) {
throttle_ = 0;
} else {
throttle_ = copysign((::std::abs(throttle) - kThrottleDeadband) /
- (1.0 - kThrottleDeadband), throttle);
+ (1.0 - kThrottleDeadband),
+ throttle);
}
- // TODO(austin): Fix the upshift logic to include states.
- Gear requested_gear;
- requested_gear = highgear ? HIGH : LOW;
-
- // Can be set to HIGH and LOW instead if we want to use simple shifting.
- const Gear shift_up = kBot3SimpleShifting ? HIGH : SHIFTING_UP;
- const Gear shift_down = kBot3SimpleShifting ? LOW : SHIFTING_DOWN;
-
- if (left_gear_ != requested_gear) {
- if (IsInGear(left_gear_)) {
- if (requested_gear == HIGH) {
- left_gear_ = shift_up;
- } else {
- left_gear_ = shift_down;
- }
- } else {
- if (requested_gear == HIGH && left_gear_ == SHIFTING_DOWN) {
- left_gear_ = SHIFTING_UP;
- } else if (requested_gear == LOW && left_gear_ == SHIFTING_UP) {
- left_gear_ = SHIFTING_DOWN;
- }
- }
- }
- if (right_gear_ != requested_gear) {
- if (IsInGear(right_gear_)) {
- if (requested_gear == HIGH) {
- right_gear_ = shift_up;
- } else {
- right_gear_ = shift_down;
- }
- } else {
- if (requested_gear == HIGH && right_gear_ == SHIFTING_DOWN) {
- right_gear_ = SHIFTING_UP;
- } else if (requested_gear == LOW && right_gear_ == SHIFTING_UP) {
- right_gear_ = SHIFTING_DOWN;
- }
- }
- }
+ Gear requested_gear = highgear ? HIGH : LOW;
+ left_gear_ = right_gear_ = requested_gear;
}
- void SetPosition(const Drivetrain::Position *position) {
+ void SetPosition(const DrivetrainQueue::Position *position) {
if (position == NULL) {
++stale_count_;
} else {
@@ -371,68 +316,6 @@
position_time_delta_ = (stale_count_ + 1) * 0.01;
stale_count_ = 0;
}
-
- if (position) {
- GearLogging gear_logging;
- // Switch to the correct controller.
- const double left_middle_shifter_position =
- (kBot3LeftDriveShifter.clear_high +
- kBot3LeftDriveShifter.clear_low) / 2.0;
- const double right_middle_shifter_position =
- (kBot3RightDriveShifter.clear_high +
- kBot3RightDriveShifter.clear_low) / 2.0;
-
- if (position->left_shifter_position < left_middle_shifter_position ||
- left_gear_ == LOW) {
- if (position->right_shifter_position < right_middle_shifter_position ||
- right_gear_ == LOW) {
- gear_logging.left_loop_high = false;
- gear_logging.right_loop_high = false;
- loop_->set_controller_index(gear_logging.controller_index = 0);
- } else {
- gear_logging.left_loop_high = false;
- gear_logging.right_loop_high = true;
- loop_->set_controller_index(gear_logging.controller_index = 1);
- }
- } else {
- if (position->right_shifter_position < right_middle_shifter_position ||
- right_gear_ == LOW) {
- gear_logging.left_loop_high = true;
- gear_logging.right_loop_high = false;
- loop_->set_controller_index(gear_logging.controller_index = 2);
- } else {
- gear_logging.left_loop_high = true;
- gear_logging.right_loop_high = true;
- loop_->set_controller_index(gear_logging.controller_index = 3);
- }
- }
-
- // TODO(austin): Constants.
- if (position->left_shifter_position >
- kBot3LeftDriveShifter.clear_high &&
- left_gear_ == SHIFTING_UP) {
- left_gear_ = HIGH;
- }
- if (position->left_shifter_position <
- kBot3LeftDriveShifter.clear_low &&
- left_gear_ == SHIFTING_DOWN) {
- left_gear_ = LOW;
- }
- if (position->right_shifter_position >
- kBot3RightDriveShifter.clear_high &&
- right_gear_ == SHIFTING_UP) {
- right_gear_ = HIGH;
- }
- if (position->right_shifter_position <
- kBot3RightDriveShifter.clear_low &&
- right_gear_ == SHIFTING_DOWN) {
- right_gear_ = LOW;
- }
-
- gear_logging.left_state = left_gear_;
- gear_logging.right_state = right_gear_;
- LOG_STRUCT(DEBUG, "state", gear_logging);
- }
}
double FilterVelocity(double throttle) {
@@ -454,9 +337,10 @@
const double adjusted_ff_voltage = ::aos::Clip(
throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
- return ((adjusted_ff_voltage +
- ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) / 2.0) /
- (ttrust_ * min_K_sum + min_FF_sum));
+ return (adjusted_ff_voltage +
+ ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
+ 2.0) /
+ (ttrust_ * min_K_sum + min_FF_sum);
}
double MaxVelocity() {
@@ -473,11 +357,11 @@
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
- //const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ // const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
const double high_min_FF_sum = FF_high.col(0).sum();
- const double adjusted_ff_voltage = ::aos::Clip(
- 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+ const double adjusted_ff_voltage =
+ ::aos::Clip(12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
return adjusted_ff_voltage / min_FF_sum;
}
@@ -485,47 +369,35 @@
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
++counter_;
+
const double current_left_velocity =
(position_.left_encoder - last_position_.left_encoder) /
position_time_delta_;
const double current_right_velocity =
(position_.right_encoder - last_position_.right_encoder) /
position_time_delta_;
- const double left_motor_speed =
- MotorSpeed(kBot3LeftDriveShifter,
- position_.left_shifter_position,
- current_left_velocity,
- left_gear_);
- const double right_motor_speed =
- MotorSpeed(kBot3RightDriveShifter,
- position_.right_shifter_position,
- current_right_velocity,
- right_gear_);
+ const double left_motor_speed = MotorSpeed(left_gear_ == HIGH,
+ current_left_velocity);
+ const double right_motor_speed = MotorSpeed(right_gear_ == HIGH,
+ current_right_velocity);
{
CIMLogging logging;
// Reset the CIM model to the current conditions to be ready for when we
// shift.
- if (IsInGear(left_gear_)) {
- logging.left_in_gear = true;
- } else {
- logging.left_in_gear = false;
- }
+ logging.left_in_gear = true;
logging.left_motor_speed = left_motor_speed;
logging.left_velocity = current_left_velocity;
- if (IsInGear(right_gear_)) {
- logging.right_in_gear = true;
- } else {
- logging.right_in_gear = false;
- }
+
+ logging.right_in_gear = true;
logging.right_motor_speed = right_motor_speed;
logging.right_velocity = current_right_velocity;
LOG_STRUCT(DEBUG, "currently", logging);
}
- if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
+ {
// FF * X = U (steady state)
const Eigen::Matrix<double, 2, 2> FF =
loop_->B().inverse() *
@@ -548,6 +420,7 @@
}
const double left_velocity = fvel - steering_velocity;
const double right_velocity = fvel + steering_velocity;
+ LOG(DEBUG, "l=%f r=%f\n", left_velocity, right_velocity);
// Integrate velocity to get the position.
// This position is used to get integral control.
@@ -565,8 +438,8 @@
U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat());
// Limit R back inside the box.
- loop_->mutable_R() =
- CoerceGoal(R_poly, equality_k, equality_w, loop_->R());
+ loop_->mutable_R() = frc971::control_loops::CoerceGoal(
+ R_poly, equality_k, equality_w, loop_->R());
}
const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
@@ -578,35 +451,18 @@
}
// TODO(austin): Model this better.
- // TODO(austin): Feed back?
+ // TODO(austin): Feedback?
loop_->mutable_X_hat() =
loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
- } else {
- // Any motor is not in gear. Speed match.
- ::Eigen::Matrix<double, 1, 1> R_left;
- ::Eigen::Matrix<double, 1, 1> R_right;
- R_left(0, 0) = left_motor_speed;
- R_right(0, 0) = right_motor_speed;
-
- const double wiggle =
- (static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
-
- loop_->mutable_U(0, 0) = ::aos::Clip(
- (R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
- -12.0, 12.0);
- loop_->mutable_U(1, 0) = ::aos::Clip(
- (R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
- -12.0, 12.0);
- loop_->mutable_U() *= 12.0 / position_.battery_voltage;
}
}
- void SendMotors(Drivetrain::Output *output) {
+ void SendMotors(DrivetrainQueue::Output *output) {
if (output != NULL) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
- output->left_high = left_gear_ == HIGH || left_gear_ == SHIFTING_UP;
- output->right_high = right_gear_ == HIGH || right_gear_ == SHIFTING_UP;
+ output->left_high = left_gear_ == HIGH;
+ output->right_high = right_gear_ == HIGH;
}
}
@@ -623,10 +479,11 @@
double position_time_delta_;
Gear left_gear_;
Gear right_gear_;
- Drivetrain::Position last_position_;
- Drivetrain::Position position_;
+ DrivetrainQueue::Position last_position_;
+ DrivetrainQueue::Position position_;
int counter_;
};
+
constexpr double PolyDrivetrain::kStallTorque;
constexpr double PolyDrivetrain::kStallCurrent;
constexpr double PolyDrivetrain::kFreeSpeed;
@@ -639,11 +496,10 @@
constexpr double PolyDrivetrain::Kv;
constexpr double PolyDrivetrain::Kt;
-
-void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
- const Drivetrain::Position *position,
- Drivetrain::Output *output,
- Drivetrain::Status * status) {
+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;
@@ -655,17 +511,22 @@
}
no_position_.Print();
- double wheel = goal->steering;
- double throttle = goal->throttle;
- bool quickturn = goal->quickturn;
- bool highgear = goal->highgear;
+ bool control_loop_driving = false;
+ if (goal) {
+ double wheel = goal->steering;
+ double throttle = goal->throttle;
+ bool quickturn = goal->quickturn;
+ bool highgear = goal->highgear;
- bool control_loop_driving = goal->control_loop_driving;
- double left_goal = goal->left_goal;
- double right_goal = goal->right_goal;
+ 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);
+ dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
+ goal->right_velocity_goal);
+ dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+ }
+
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
@@ -678,7 +539,6 @@
}
}
dt_openloop.SetPosition(position);
- dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
if (control_loop_driving) {
@@ -699,15 +559,18 @@
if (goal) {
done = ((::std::abs(goal->left_goal -
dt_closedloop.GetEstimatedLeftEncoder()) <
- kBot3DrivetrainDoneDistance) &&
+ kDrivetrainDoneDistance) &&
(::std::abs(goal->right_goal -
dt_closedloop.GetEstimatedRightEncoder()) <
- kBot3DrivetrainDoneDistance));
+ kDrivetrainDoneDistance));
}
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);
@@ -715,4 +578,4 @@
}
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.gyp b/y2014_bot3/control_loops/drivetrain/drivetrain.gyp
deleted file mode 100644
index d17cdf2..0000000
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.gyp
+++ /dev/null
@@ -1,102 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'drivetrain_loop',
- 'type': 'static_library',
- 'sources': ['drivetrain.q'],
- 'variables': {
- 'header_path': 'bot3/control_loops/drivetrain',
- },
- 'dependencies': [
- '<(AOS)/common/controls/controls.gyp:control_loop_queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/controls/controls.gyp:control_loop_queues',
- ],
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'polydrivetrain_plants',
- 'type': 'static_library',
- 'sources': [
- 'polydrivetrain_dog_motor_plant.cc',
- 'drivetrain_dog_motor_plant.cc',
- ],
- 'dependencies': [
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- },
- {
- 'target_name': 'drivetrain_constants',
- 'type': 'static_library',
- 'sources': [
- #'drivetrain_constants.h'
- ],
- 'dependencies': [
- 'polydrivetrain_plants',
- ],
- 'export_dependent_settings': [
- 'polydrivetrain_plants',
- ],
- },
- {
- 'target_name': 'drivetrain_lib',
- 'type': 'static_library',
- 'sources': [
- 'drivetrain.cc',
- 'polydrivetrain_cim_plant.cc',
- ],
- 'dependencies': [
- 'drivetrain_loop',
- 'drivetrain_constants',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- '<(AOS)/common/controls/controls.gyp:polytope',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/util/util.gyp:log_interval',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(AOS)/common/logging/logging.gyp:matrix_logging',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/controls/controls.gyp:polytope',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- 'drivetrain_loop',
- ],
- },
- {
- 'target_name': 'drivetrain_lib_test',
- 'type': 'executable',
- 'sources': [
- 'drivetrain_lib_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- 'drivetrain_loop',
- 'drivetrain_lib',
- '<(AOS)/common/controls/controls.gyp:control_loop_test',
- '<(AOS)/common/network/network.gyp:team_number',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- },
- {
- 'target_name': 'drivetrain',
- 'type': 'executable',
- 'sources': [
- 'drivetrain_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'drivetrain_lib',
- 'drivetrain_loop',
- ],
- },
- ],
-}
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.h b/y2014_bot3/control_loops/drivetrain/drivetrain.h
index b2bed8c..0565820 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.h
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.h
@@ -1,5 +1,5 @@
-#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
-#define BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
#include "Eigen/Dense"
@@ -7,30 +7,46 @@
#include "aos/common/controls/control_loop.h"
#include "aos/common/controls/polytope.h"
#include "aos/common/util/log_interval.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-namespace bot3 {
+#include "frc971/shifter_hall_effect.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+
+namespace y2014_bot3 {
namespace control_loops {
+// Constants
+// TODO(comran): Get actual constants.
+constexpr double kDrivetrainTurnWidth = 0.63;
+constexpr double kDrivetrainDoneDistance = 0.02;
+constexpr double kDrivetrainEncoderRatio = 18.0 / 44.0;
+constexpr double kDrivetrainHighGearRatio =
+ kDrivetrainEncoderRatio * 18.0 / 60.0;
+constexpr double kDrivetrainLowGearRatio = kDrivetrainHighGearRatio;
+const ::frc971::constants::ShifterHallEffect kDrivetrainRightShifter{
+ 555, 657, 660, 560, 0.2, 0.7};
+const ::frc971::constants::ShifterHallEffect kDrivetrainLeftShifter{
+ 555, 660, 644, 552, 0.2, 0.7};
+// End constants
+
class DrivetrainLoop
- : public aos::controls::ControlLoop<control_loops::Drivetrain, true, false> {
+ : public aos::controls::ControlLoop<control_loops::DrivetrainQueue> {
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
- // drivetrain at bot3::control_loops::drivetrain
- explicit DrivetrainLoop(
- control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
- : aos::controls::ControlLoop<control_loops::Drivetrain, true, false>(
- my_drivetrain) {
+ // drivetrain at y2014_bot3::control_loops::drivetrain
+ explicit DrivetrainLoop(control_loops::DrivetrainQueue *my_drivetrain =
+ &control_loops::drivetrain_queue)
+ : aos::controls::ControlLoop<control_loops::DrivetrainQueue>(
+ my_drivetrain) {
::aos::controls::HPolytope<0>::Init();
}
protected:
// Executes one cycle of the control loop.
virtual void RunIteration(
- const control_loops::Drivetrain::Goal *goal,
- const control_loops::Drivetrain::Position *position,
- control_loops::Drivetrain::Output *output,
- control_loops::Drivetrain::Status *status);
+ const control_loops::DrivetrainQueue::Goal *goal,
+ const control_loops::DrivetrainQueue::Position *position,
+ control_loops::DrivetrainQueue::Output *output,
+ control_loops::DrivetrainQueue::Status *status);
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
SimpleLogInterval no_position_ = SimpleLogInterval(
@@ -38,6 +54,6 @@
};
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.q b/y2014_bot3/control_loops/drivetrain/drivetrain.q
index f4fe3d5..c4563f9 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.q
@@ -1,15 +1,7 @@
-package bot3.control_loops;
+package y2014_bot3.control_loops;
import "aos/common/controls/control_loops.q";
-struct GearLogging {
- int8_t controller_index;
- bool left_loop_high;
- bool right_loop_high;
- int8_t left_state;
- int8_t right_state;
-};
-
struct CIMLogging {
bool left_in_gear;
bool right_in_gear;
@@ -19,7 +11,7 @@
double right_velocity;
};
-queue_group Drivetrain {
+queue_group DrivetrainQueue {
implements aos.control_loops.ControlLoop;
message Goal {
@@ -37,9 +29,6 @@
message Position {
double left_encoder;
double right_encoder;
- double left_shifter_position;
- double right_shifter_position;
- double battery_voltage;
};
message Output {
@@ -53,6 +42,8 @@
double robot_speed;
double filtered_left_position;
double filtered_right_position;
+ double filtered_left_velocity;
+ double filtered_right_velocity;
double uncapped_left_voltage;
double uncapped_right_voltage;
@@ -67,4 +58,4 @@
queue Status status;
};
-queue_group Drivetrain drivetrain;
+queue_group DrivetrainQueue drivetrain_queue;
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index da59426..05f6a2b 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -1,17 +1,17 @@
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include <vector>
#include "frc971/control_loops/state_feedback_loop.h"
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00718627304278, 0.0, 0.000575327102319, 0.0, 0.498791664199, 0.0, 0.0897184892451, 0.0, 0.000575327102319, 1.0, 0.00718627304278, 0.0, 0.0897184892451, 0.0, 0.498791664199;
+ A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000404687213064, -8.27470202964e-05, 0.0720868114296, -0.0129038552514, -8.27470202964e-05, 0.000404687213064, -0.0129038552514, 0.0720868114296;
+ B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00715806931946, 0.0, 8.27195198739e-05, 0.0, 0.491841728751, 0.0, 0.0145551608494, 0.0, 0.000706303878161, 1.0, 0.00959963582133, 0.0, 0.124279814134, 0.0, 0.921454874639;
+ A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000408743642973, -3.47646073582e-05, 0.0730863931411, -0.00611711060145, -0.000101584891631, 0.000168261415116, -0.0178746738354, 0.0330102308046;
+ B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00959963582133, 0.0, 0.000706303878161, 0.0, 0.921454874639, 0.0, 0.124279814134, 0.0, 8.27195198739e-05, 1.0, 0.00715806931946, 0.0, 0.0145551608494, 0.0, 0.491841728751;
+ A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000168261415116, -0.000101584891631, 0.0330102308046, -0.0178746738354, -3.47646073582e-05, 0.000408743642973, -0.00611711060145, 0.0730863931411;
+ B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00959512220091, 0.0, 0.000100752776744, 0.0, 0.920202836632, 0.0, 0.0195951135, 0.0, 0.000100752776744, 1.0, 0.00959512220091, 0.0, 0.0195951135, 0.0, 0.920202836632;
+ A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000170158358449, -4.23434605168e-05, 0.0335364259492, -0.00823525605573, -4.23434605168e-05, 0.000170158358449, -0.00823525605573, 0.0335364259492;
+ B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,53 +73,61 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 0.798791664199, 0.0897184892451, 3.57844915691, 3.42875197165, 0.0897184892451, 0.798791664199, 3.42875197165, 3.57844915691;
+ L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
Eigen::Matrix<double, 2, 4> K;
- K << 118.526561433, 4.19900632703, 7.79020074229, 0.921948843301, 7.79020074228, 0.921948843301, 118.526561433, 4.19900632703;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowLowPlantCoefficients());
+ K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 0.780361183952, 0.0157698223449, 2.31127571816, 1.09059511674, 0.0157698223449, 1.23293541944, 3.02157734826, 34.5051463402;
+ L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
Eigen::Matrix<double, 2, 4> K;
- K << 116.74766142, 4.04674599343, -6.38397728262, -0.174173971494, 20.8480994015, 2.49474383494, 135.799605924, 11.0272286082;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowHighPlantCoefficients());
+ K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.26735275667, 0.0994717602348, 37.3766125148, 7.14618746927, 0.0994717602348, 0.745943846725, 2.20133516364, 1.9922178483;
+ L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
Eigen::Matrix<double, 2, 4> K;
- K << 135.799605924, 11.0272286082, 20.8480994015, 2.49474383494, -6.38397728263, -0.174173971495, 116.74766142, 4.04674599343;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighLowPlantCoefficients());
+ K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.22020283663, 0.0195951135, 33.6437810428, 1.9756574066, 0.0195951135, 1.22020283663, 1.9756574066, 33.6437810428;
+ L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
Eigen::Matrix<double, 2, 4> K;
- K << 136.125709167, 11.0402255404, 6.0764292977, 1.24911698877, 6.0764292977, 1.24911698877, 136.125709167, 11.0402255404;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighHighPlantCoefficients());
+ K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
}
StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<4, 2, 2>(plants);
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(4);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients()));
+ plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients()));
+ plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients()));
+ plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients()));
+ return StateFeedbackPlant<4, 2, 2>(&plants);
}
StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
- ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController());
- return StateFeedbackLoop<4, 2, 2>(controllers);
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(4);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController()));
+ controllers[1] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController()));
+ controllers[2] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController()));
+ controllers[3] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController()));
+ return StateFeedbackLoop<4, 2, 2>(&controllers);
}
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
index f5d5896..03322ed 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -1,9 +1,9 @@
-#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients();
@@ -27,6 +27,6 @@
StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
index 89e41c6..2c04831 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -8,15 +8,14 @@
#include "aos/common/controls/polytope.h"
#include "aos/common/controls/control_loop_test.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain.h"
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "frc971/queues/gyro.q.h"
-
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
namespace testing {
@@ -49,11 +48,11 @@
DrivetrainSimulation()
: drivetrain_plant_(
new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
- my_drivetrain_loop_(".frc971.control_loops.drivetrain",
- 0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
- ".frc971.control_loops.drivetrain.position",
- ".frc971.control_loops.drivetrain.output",
- ".frc971.control_loops.drivetrain.status") {
+ my_drivetrain_queue_(".y2014_bot3.control_loops.drivetrain",
+ 0x8a8dde77, ".y2014_bot3.control_loops.drivetrain.goal",
+ ".y2014_bot3.control_loops.drivetrain.position",
+ ".y2014_bot3.control_loops.drivetrain.output",
+ ".y2014_bot3.control_loops.drivetrain.status") {
Reinitialize();
}
@@ -76,8 +75,8 @@
const double left_encoder = GetLeftPosition();
const double right_encoder = GetRightPosition();
- ::aos::ScopedMessagePtr<control_loops::Drivetrain::Position> position =
- my_drivetrain_loop_.position.MakeMessage();
+ ::aos::ScopedMessagePtr<control_loops::DrivetrainQueue::Position> position =
+ my_drivetrain_queue_.position.MakeMessage();
position->left_encoder = left_encoder;
position->right_encoder = right_encoder;
position.Send();
@@ -87,15 +86,15 @@
void Simulate() {
last_left_position_ = drivetrain_plant_->Y(0, 0);
last_right_position_ = drivetrain_plant_->Y(1, 0);
- EXPECT_TRUE(my_drivetrain_loop_.output.FetchLatest());
- drivetrain_plant_->mutable_U() << my_drivetrain_loop_.output->left_voltage,
- my_drivetrain_loop_.output->right_voltage;
+ EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ drivetrain_plant_->mutable_U() << my_drivetrain_queue_.output->left_voltage,
+ my_drivetrain_queue_.output->right_voltage;
drivetrain_plant_->Update();
}
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
private:
- Drivetrain my_drivetrain_loop_;
+ DrivetrainQueue my_drivetrain_queue_;
double last_left_position_;
double last_right_position_;
};
@@ -105,30 +104,30 @@
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointer to shared memory that
// is no longer valid.
- Drivetrain my_drivetrain_loop_;
+ DrivetrainQueue my_drivetrain_queue_;
// Create a loop and simulation plant.
DrivetrainLoop drivetrain_motor_;
DrivetrainSimulation drivetrain_motor_plant_;
- DrivetrainTest() : my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+ DrivetrainTest() : my_drivetrain_queue_(".y2014_bot3.control_loops.drivetrain",
0x8a8dde77,
- ".frc971.control_loops.drivetrain.goal",
- ".frc971.control_loops.drivetrain.position",
- ".frc971.control_loops.drivetrain.output",
- ".frc971.control_loops.drivetrain.status"),
- drivetrain_motor_(&my_drivetrain_loop_),
+ ".y2014_bot3.control_loops.drivetrain.goal",
+ ".y2014_bot3.control_loops.drivetrain.position",
+ ".y2014_bot3.control_loops.drivetrain.output",
+ ".y2014_bot3.control_loops.drivetrain.status"),
+ drivetrain_motor_(&my_drivetrain_queue_),
drivetrain_motor_plant_() {
::frc971::sensors::gyro_reading.Clear();
}
void VerifyNearGoal() {
- my_drivetrain_loop_.goal.FetchLatest();
- my_drivetrain_loop_.position.FetchLatest();
- EXPECT_NEAR(my_drivetrain_loop_.goal->left_goal,
+ my_drivetrain_queue_.goal.FetchLatest();
+ my_drivetrain_queue_.position.FetchLatest();
+ EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
drivetrain_motor_plant_.GetLeftPosition(),
1e-2);
- EXPECT_NEAR(my_drivetrain_loop_.goal->right_goal,
+ EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
drivetrain_motor_plant_.GetRightPosition(),
1e-2);
}
@@ -140,7 +139,7 @@
// Tests that the drivetrain converges on a goal.
TEST_F(DrivetrainTest, ConvergesCorrectly) {
- my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
.left_goal(-1.0)
.right_goal(1.0).Send();
for (int i = 0; i < 200; ++i) {
@@ -154,7 +153,7 @@
// Tests that it survives disabling.
TEST_F(DrivetrainTest, SurvivesDisabling) {
- my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
.left_goal(-1.0)
.right_goal(1.0).Send();
for (int i = 0; i < 500; ++i) {
@@ -170,21 +169,24 @@
VerifyNearGoal();
}
-// Tests surviving bad positions.
-TEST_F(DrivetrainTest, SurvivesBadPosition) {
- my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
- .left_goal(-1.0)
- .right_goal(1.0).Send();
- for (int i = 0; i < 500; ++i) {
- if (i > 20 && i < 200) {
- } else {
- drivetrain_motor_plant_.SendPositionMessage();
- }
+// Tests that never having a goal doesn't break.
+TEST_F(DrivetrainTest, NoGoalStart) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ }
+}
+
+// Tests that never having a goal, but having driver's station messages, doesn't
+// break.
+TEST_F(DrivetrainTest, NoGoalWithRobotState) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
SimulateTimestep(true);
}
- VerifyNearGoal();
}
::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
@@ -291,4 +293,4 @@
} // namespace testing
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
index f06acc0..588bc89 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -1,10 +1,10 @@
-#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
#include "aos/linux_code/init.h"
int main() {
::aos::Init();
- bot3::control_loops::DrivetrainLoop drivetrain;
+ y2014_bot3::control_loops::DrivetrainLoop drivetrain;
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
index 1287483..75bae13 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
@@ -1,17 +1,17 @@
-#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include <vector>
#include "frc971/control_loops/state_feedback_loop.h"
-namespace frc971 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients() {
Eigen::Matrix<double, 1, 1> A;
- A << 0.614537580221;
+ A << 0.783924473544;
Eigen::Matrix<double, 1, 1> B;
- B << 15.9657598852;
+ B << 8.94979586973;
Eigen::Matrix<double, 1, 1> C;
C << 1;
Eigen::Matrix<double, 1, 1> D;
@@ -25,23 +25,25 @@
StateFeedbackController<1, 1, 1> MakeCIMController() {
Eigen::Matrix<double, 1, 1> L;
- L << 0.604537580221;
+ L << 0.773924473544;
Eigen::Matrix<double, 1, 1> K;
- K << 0.0378646293422;
- return StateFeedbackController<1, 1, 1>(L, K, MakeCIMPlantCoefficients());
+ K << 0.086473980503;
+ Eigen::Matrix<double, 1, 1> A_inv;
+ A_inv << 1.2756330919;
+ return StateFeedbackController<1, 1, 1>(L, K, A_inv, MakeCIMPlantCoefficients());
}
StateFeedbackPlant<1, 1, 1> MakeCIMPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<1, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients());
- return StateFeedbackPlant<1, 1, 1>(plants);
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>(new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients()));
+ return StateFeedbackPlant<1, 1, 1>(&plants);
}
StateFeedbackLoop<1, 1, 1> MakeCIMLoop() {
- ::std::vector<StateFeedbackController<1, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<1, 1, 1>(MakeCIMController());
- return StateFeedbackLoop<1, 1, 1>(controllers);
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<1, 1, 1>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<1, 1, 1>>(new StateFeedbackController<1, 1, 1>(MakeCIMController()));
+ return StateFeedbackLoop<1, 1, 1>(&controllers);
}
} // namespace control_loops
-} // namespace frc971
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
index 12b2c59..0def071 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
@@ -1,9 +1,9 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
-namespace frc971 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients();
@@ -15,6 +15,6 @@
StateFeedbackLoop<1, 1, 1> MakeCIMLoop();
} // namespace control_loops
-} // namespace frc971
+} // namespace y2014_bot3
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index 540741c..8fc4049 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -1,17 +1,17 @@
-#include "bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
#include <vector>
#include "frc971/control_loops/state_feedback_loop.h"
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.498791664199, 0.0897184892451, 0.0897184892451, 0.498791664199;
+ A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0720868114296, -0.0129038552514, -0.0129038552514, 0.0720868114296;
+ B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.491841728751, 0.0145551608494, 0.124279814134, 0.921454874639;
+ A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0730863931411, -0.00611711060145, -0.0178746738354, 0.0330102308046;
+ B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.921454874639, 0.124279814134, 0.0145551608494, 0.491841728751;
+ A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0330102308046, -0.0178746738354, -0.00611711060145, 0.0730863931411;
+ B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.920202836632, 0.0195951135, 0.0195951135, 0.920202836632;
+ A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0335364259492, -0.00823525605573, -0.00823525605573, 0.0335364259492;
+ B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,53 +73,61 @@
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.478791664199, 0.0897184892451, 0.0897184892451, 0.478791664199;
+ L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
Eigen::Matrix<double, 2, 2> K;
- K << -1.22029287467, 1.0261517351, 1.0261517351, -1.22029287467;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
+ K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.450790078471, 0.12008012565, 0.12008012565, 0.922506524919;
+ L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
Eigen::Matrix<double, 2, 2> K;
- K << -1.22005303899, 1.06234079364, 3.10424257883, 10.3132835345;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
+ K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.918551885829, 0.0172871978996, 0.0172871978996, 0.454744717561;
+ L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
Eigen::Matrix<double, 2, 2> K;
- K << 10.3132835345, 3.10424257883, 1.06234079364, -1.22005303899;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
+ K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.900202836632, 0.0195951135, 0.0195951135, 0.900202836632;
+ L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
Eigen::Matrix<double, 2, 2> K;
- K << 10.3132878272, 3.11684016931, 3.11684016931, 10.3132878272;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
+ K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<2, 2, 2>(plants);
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>> plants(4);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients()));
+ plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients()));
+ plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients()));
+ plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients()));
+ return StateFeedbackPlant<2, 2, 2>(&plants);
}
StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
- ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
- return StateFeedbackLoop<2, 2, 2>(controllers);
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 2, 2>>> controllers(4);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController()));
+ controllers[1] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController()));
+ controllers[2] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController()));
+ controllers[3] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController()));
+ return StateFeedbackLoop<2, 2, 2>(&controllers);
}
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
index 191d1aa..4442d35 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
@@ -1,9 +1,9 @@
-#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
@@ -27,6 +27,6 @@
StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop();
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2014_bot3/control_loops/drivetrain/replay_drivetrain.cc b/y2014_bot3/control_loops/drivetrain/replay_drivetrain.cc
new file mode 100644
index 0000000..4f83c9e
--- /dev/null
+++ b/y2014_bot3/control_loops/drivetrain/replay_drivetrain.cc
@@ -0,0 +1,24 @@
+#include "aos/common/controls/replay_control_loop.h"
+#include "aos/linux_code/init.h"
+
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+
+// Reads one or more log files and sends out all the queue messages (in the
+// correct order and at the correct time) to feed a "live" drivetrain process.
+
+int main(int argc, char **argv) {
+ if (argc <= 1) {
+ fprintf(stderr, "Need at least one file to replay!\n");
+ return EXIT_FAILURE;
+ }
+
+ ::aos::InitNRT();
+
+ ::aos::controls::ControlLoopReplayer<::y2014_bot3::control_loops::DrivetrainQueue>
+ replayer(&::y2014_bot3::control_loops::drivetrain_queue, "drivetrain");
+ for (int i = 1; i < argc; ++i) {
+ replayer.ProcessFile(argv[i]);
+ }
+
+ ::aos::Cleanup();
+}
diff --git a/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index bc0df04..c0c4595 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -56,30 +56,33 @@
# Stall Torque in N m
self.stall_torque = 2.42
# Stall Current in Amps
- self.stall_current = 133
+ self.stall_current = 133.0
# Free Speed in RPM. Used number from last year.
self.free_speed = 4650.0
# Free Current in Amps
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 4.5
+ self.J = 10
# Mass of the robot, in kg.
- self.m = 60
+ self.m = 68
# Radius of the robot, in meters (from last year).
- self.rb = 0.7112 / 2.0
+ self.rb = 0.9603 / 2.0
# Radius of the wheels, in meters.
- self.r = .04445
+ self.r = 0.0508
# Resistance of the motor, divided by the number of motors.
- self.R = 12.0 / self.stall_current / 4
+ self.R = 12.0 / self.stall_current / 2
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
- self.G_low = 14.0 / 60.0 * 17.0 / 50.0
- self.G_high = 30.0 / 44.0 * 17.0 / 50.0
+ self.G_const = 18.0 / 44.0 * 18.0 / 60.0
+
+ self.G_low = self.G_const
+ self.G_high = self.G_const
+
if left_low:
self.Gl = self.G_low
else:
@@ -88,8 +91,9 @@
self.Gr = self.G_low
else:
self.Gr = self.G_high
+
# Control loop time step
- self.dt = 0.01
+ self.dt = 0.005
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
@@ -223,13 +227,13 @@
drivetrain_high_low = Drivetrain(name="DrivetrainHighLow", left_low=False, right_low=True)
drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh", left_low=False, right_low=False)
- if len(argv) != 3:
+ if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
"Drivetrain", [drivetrain_low_low, drivetrain_low_high,
drivetrain_high_low, drivetrain_high_high],
- namespaces = ["bot3", "control_loops"])
+ namespaces=['y2014_bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index 49d390b..2ffbed6 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -102,7 +102,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ self.dt = 0.005
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
[self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
@@ -168,7 +168,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
@@ -177,7 +177,7 @@
# ttrust is the comprimise between having full throttle negative inertia,
# and having no throttle negative inertia. A value of 0 is full throttle
# inertia. A value of 1 is no throttle negative inertia.
- self.ttrust = 1.0
+ self.ttrust = 1.1
self.left_gear = VelocityDrivetrain.LOW
self.right_gear = VelocityDrivetrain.LOW
@@ -302,26 +302,30 @@
# This is the same as sending the most torque down to the floor at the
# wheel.
- self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
- current_gear=self.left_gear,
- gear_name="left")
- self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
- current_gear=self.right_gear,
- gear_name="right")
- if self.IsInGear(self.left_gear):
- self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.left_gear = self.right_gear = True
+ if False:
+ self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
- if self.IsInGear(self.right_gear):
- self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ steering *= 2.3
+ if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
# Filter the throttle to provide a nicer response.
fvel = self.FilterVelocity(throttle)
# Constant radius means that angualar_velocity / linear_velocity = constant.
# Compute the left and right velocities.
- left_velocity = fvel - steering * numpy.abs(fvel)
- right_velocity = fvel + steering * numpy.abs(fvel)
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
# Write this constraint in the form of K * R = w
# angular velocity / linear velocity = constant
@@ -379,7 +383,7 @@
# TODO(austin): Model the robot as not accelerating when you shift...
# This hack only works when you shift at the same time.
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
self.left_gear, self.left_shifter_position = self.SimShifter(
@@ -394,7 +398,7 @@
def main(argv):
vdrivetrain = VelocityDrivetrain()
- if len(argv) != 5:
+ if len(argv) != 7:
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
@@ -402,7 +406,7 @@
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
vdrivetrain.drivetrain_high_high],
- namespaces = ["bot3", "control_loops"])
+ namespaces=['y2014_bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
@@ -410,12 +414,13 @@
dog_loop_writer.Write(argv[1], argv[2])
cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ "CIM", [drivetrain.CIM()],
+ namespaces=['y2014_bot3', 'control_loops'])
- if argv[3][-3:] == '.cc':
- cim_writer.Write(argv[4], argv[3])
+ if argv[5][-3:] == '.cc':
+ cim_writer.Write(argv[6], argv[5])
else:
- cim_writer.Write(argv[3], argv[4])
+ cim_writer.Write(argv[5], argv[6])
return
vl_plot = []
@@ -442,13 +447,13 @@
else:
print "Right is low"
- for t in numpy.arange(0, 4.0, vdrivetrain.dt):
- if t < 1.0:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
elif t < 1.2:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
else:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
t_plot.append(t)
vl_plot.append(vdrivetrain.X[0, 0])
vr_plot.append(vdrivetrain.X[1, 0])
@@ -475,10 +480,10 @@
cim_velocity_plot.append(cim.X[0, 0])
cim_voltage_plot.append(U[0, 0] * 10)
cim_time.append(t)
- #pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
- #pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
- #pylab.legend()
- #pylab.show()
+ pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
+ pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
+ pylab.legend()
+ pylab.show()
# TODO(austin):
# Shifting compensation.
diff --git a/y2014_bot3/control_loops/rollers/BUILD b/y2014_bot3/control_loops/rollers/BUILD
new file mode 100644
index 0000000..e3526ac
--- /dev/null
+++ b/y2014_bot3/control_loops/rollers/BUILD
@@ -0,0 +1,40 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'rollers_queue',
+ srcs = [
+ 'rollers.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ '//frc971/control_loops:queues',
+ ],
+)
+
+cc_library(
+ name = 'rollers_lib',
+ hdrs = [
+ 'rollers.h',
+ ],
+ srcs = [
+ 'rollers.cc',
+ ],
+ deps = [
+ ':rollers_queue',
+ '//aos/common/logging',
+ '//aos/common/controls:control_loop',
+ ],
+)
+
+cc_binary(
+ name = 'rollers',
+ srcs = [
+ 'rollers_main.cc',
+ ],
+ deps = [
+ ':rollers_lib',
+ '//aos/linux_code:init',
+ ],
+)
diff --git a/y2014_bot3/control_loops/rollers/rollers.cc b/y2014_bot3/control_loops/rollers/rollers.cc
index 8777549..ef895b3 100644
--- a/y2014_bot3/control_loops/rollers/rollers.cc
+++ b/y2014_bot3/control_loops/rollers/rollers.cc
@@ -1,40 +1,45 @@
-#include "bot3/control_loops/rollers/rollers.h"
-#include "bot3/control_loops/rollers/rollers.q.h"
+#include "y2014_bot3/control_loops/rollers/rollers.h"
-namespace bot3 {
+#include "aos/common/logging/logging.h"
+
+namespace y2014_bot3 {
namespace control_loops {
-void RollersLoop::RunIteration(const Rollers::Goal *goal,
- const Rollers::Position * /*position*/,
- Rollers::Output *output,
- Rollers::Status * /*status*/) {
- constexpr double kBot3IntakeForwardVoltage = 12.0;
- constexpr double kBot3IntakeBackwardVoltage = -12.0;
- constexpr double kBot3LowGoalForwardVoltage = 6.0;
- constexpr double kBot3LowGoalBackwardVoltage = -6.0;
+Rollers::Rollers(control_loops::RollersQueue *rollers)
+ : aos::controls::ControlLoop<control_loops::RollersQueue>(rollers) {}
+
+void Rollers::RunIteration(
+ const control_loops::RollersQueue::Goal *goal,
+ const control_loops::RollersQueue::Position * /*position*/,
+ control_loops::RollersQueue::Output *output,
+ control_loops::RollersQueue::Status * /*status*/) {
+ constexpr double k2014Bot3IntakeForwardVoltage = 12.0;
+ constexpr double k2014Bot3IntakeBackwardVoltage = -12.0;
+ constexpr double k2014Bot3LowGoalForwardVoltage = 6.0;
+ constexpr double k2014Bot3LowGoalBackwardVoltage = -6.0;
+
+ if (!output || !goal) {
+ return;
+ }
const int intake = goal->intake;
const int low_spit = goal->low_spit;
const bool human_player = goal->human_player;
- if (!output) {
- return;
- }
-
output->Zero();
switch (low_spit) {
case 1:
// Spit towards front
- output->low_goal_voltage = kBot3LowGoalBackwardVoltage;
- output->front_intake_voltage = kBot3IntakeBackwardVoltage;
- output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+ output->low_goal_voltage = k2014Bot3LowGoalBackwardVoltage;
+ output->front_intake_voltage = k2014Bot3IntakeBackwardVoltage;
+ output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
break;
case -1:
// Spit towards back
- output->low_goal_voltage = kBot3LowGoalForwardVoltage;
- output->back_intake_voltage = -kBot3IntakeBackwardVoltage;
- output->front_intake_voltage = kBot3IntakeForwardVoltage;
+ output->low_goal_voltage = k2014Bot3LowGoalForwardVoltage;
+ output->back_intake_voltage = -k2014Bot3IntakeBackwardVoltage;
+ output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
break;
default:
// Stationary
@@ -46,14 +51,14 @@
// Front intake.
output->front_extended = true;
output->back_extended = false;
- output->front_intake_voltage = kBot3IntakeForwardVoltage;
+ output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
output->back_intake_voltage = 0.0;
break;
case -1:
// Back intake.
output->back_extended = true;
output->front_extended = false;
- output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+ output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
output->front_intake_voltage = 0.0;
break;
default:
@@ -65,10 +70,10 @@
// Intake for human player.
output->front_extended = false;
output->back_extended = false;
- output->front_intake_voltage = kBot3IntakeForwardVoltage;
- output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+ output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
+ output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
}
}
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/rollers/rollers.gyp b/y2014_bot3/control_loops/rollers/rollers.gyp
deleted file mode 100644
index e865ed8..0000000
--- a/y2014_bot3/control_loops/rollers/rollers.gyp
+++ /dev/null
@@ -1,45 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'rollers_loop',
- 'type': 'static_library',
- 'sources': ['rollers.q'],
- 'variables': {
- 'header_path': 'bot3/control_loops/rollers',
- },
- 'dependencies': [
- '<(AOS)/common/controls/controls.gyp:control_loop_queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/controls/controls.gyp:control_loop_queues',
- ],
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'rollers_lib',
- 'type': 'static_library',
- 'sources': [
- 'rollers.cc',
- ],
- 'dependencies': [
- 'rollers_loop',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- ],
- 'export_dependent_settings': [
- 'rollers_loop',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- ],
- },
- {
- 'target_name': 'rollers',
- 'type': 'executable',
- 'sources': [
- 'rollers_main.cc',
- ],
- 'dependencies': [
- 'rollers_lib',
- '<(AOS)/linux_code/linux_code.gyp:init'
- ],
- },
- ],
-}
diff --git a/y2014_bot3/control_loops/rollers/rollers.h b/y2014_bot3/control_loops/rollers/rollers.h
index f67b372..f0cae13 100644
--- a/y2014_bot3/control_loops/rollers/rollers.h
+++ b/y2014_bot3/control_loops/rollers/rollers.h
@@ -1,33 +1,29 @@
-#ifndef BOT3_CONTROL_LOOPS_ROLLERS_H_
-#define BOT3_CONTROL_LOOPS_ROLLERS_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_ROLLERS_H_
+#define Y2014_BOT3_CONTROL_LOOPS_ROLLERS_H_
#include "aos/common/controls/control_loop.h"
-#include "bot3/control_loops/rollers/rollers.q.h"
-namespace bot3 {
+#include "y2014_bot3/control_loops/rollers/rollers.q.h"
+
+namespace y2014_bot3 {
namespace control_loops {
-class RollersLoop
- : public aos::controls::ControlLoop<control_loops::Rollers,
- false, false, true> {
+class Rollers : public aos::controls::ControlLoop<control_loops::RollersQueue> {
public:
// Constructs a control loops which can take a rollers or defaults to the
- // rollers at ::bot3::control_loops::rollers.
- explicit RollersLoop(
- control_loops::Rollers *my_rollers = &control_loops::rollers)
- : aos::controls::ControlLoop<control_loops::Rollers, false, false, true>(
- my_rollers) {}
+ // rollers at ::2014_bot3::control_loops::rollers.
+ explicit Rollers(control_loops::RollersQueue *rollers_queue =
+ &control_loops::rollers_queue);
protected:
// Executes one cycle of the control loop.
- virtual void RunIteration(
- const control_loops::Rollers::Goal *goal,
- const control_loops::Rollers::Position *position,
- control_loops::Rollers::Output *output,
- control_loops::Rollers::Status *status);
+ void RunIteration(const control_loops::RollersQueue::Goal *goal,
+ const control_loops::RollersQueue::Position *position,
+ control_loops::RollersQueue::Output *output,
+ control_loops::RollersQueue::Status *status) override;
};
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_CONTROL_LOOPS_ROLLERS_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_ROLLERS_H_
diff --git a/y2014_bot3/control_loops/rollers/rollers.q b/y2014_bot3/control_loops/rollers/rollers.q
index 5abb488..f218a41 100644
--- a/y2014_bot3/control_loops/rollers/rollers.q
+++ b/y2014_bot3/control_loops/rollers/rollers.q
@@ -1,8 +1,9 @@
-package bot3.control_loops;
+package y2014_bot3.control_loops;
import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
-queue_group Rollers {
+queue_group RollersQueue {
implements aos.control_loops.ControlLoop;
message Goal {
@@ -37,4 +38,4 @@
queue Status status;
};
-queue_group Rollers rollers;
+queue_group RollersQueue rollers_queue;
diff --git a/y2014_bot3/control_loops/rollers/rollers_main.cc b/y2014_bot3/control_loops/rollers/rollers_main.cc
index 75d2cc6..4700a6e 100644
--- a/y2014_bot3/control_loops/rollers/rollers_main.cc
+++ b/y2014_bot3/control_loops/rollers/rollers_main.cc
@@ -1,10 +1,10 @@
-#include "bot3/control_loops/rollers/rollers.h"
+#include "y2014_bot3/control_loops/rollers/rollers.h"
#include "aos/linux_code/init.h"
int main() {
::aos::Init();
- bot3::control_loops::RollersLoop rollers;
+ ::y2014_bot3::control_loops::Rollers rollers;
rollers.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014_bot3/control_loops/update_drivetrain.sh b/y2014_bot3/control_loops/update_drivetrain.sh
index 91ff838..b5757d3 100755
--- a/y2014_bot3/control_loops/update_drivetrain.sh
+++ b/y2014_bot3/control_loops/update_drivetrain.sh
@@ -1,9 +1,12 @@
#!/bin/bash
#
-# Updates the drivetrain controllers for the third robot.
+# Updates the drivetrain controllers.
cd $(dirname $0)
-PYTHONPATH=$PYTHONPATH:../../frc971/control_loops/python \
+export PYTHONPATH=../../frc971/control_loops/python
+
./python/drivetrain.py drivetrain/drivetrain_dog_motor_plant.h \
- drivetrain/drivetrain_dog_motor_plant.cc
+ drivetrain/drivetrain_dog_motor_plant.cc \
+ drivetrain/drivetrain_clutch_motor_plant.h \
+ drivetrain/drivetrain_clutch_motor_plant.cc
diff --git a/y2014_bot3/control_loops/update_polydrivetrain.sh b/y2014_bot3/control_loops/update_polydrivetrain.sh
index d8e2d7a..b1bb2b4 100755
--- a/y2014_bot3/control_loops/update_polydrivetrain.sh
+++ b/y2014_bot3/control_loops/update_polydrivetrain.sh
@@ -1,11 +1,14 @@
#!/bin/bash
#
-# Updates the polydrivetrain controllers and CIM models for the third robot.
+# Updates the polydrivetrain controllers and CIM models.
cd $(dirname $0)
-PYTHONPATH=$PYTHONPATH:../../frc971/control_loops/python \
+export PYTHONPATH=../../frc971/control_loops/python
+
./python/polydrivetrain.py drivetrain/polydrivetrain_dog_motor_plant.h \
drivetrain/polydrivetrain_dog_motor_plant.cc \
+ drivetrain/polydrivetrain_clutch_motor_plant.h \
+ drivetrain/polydrivetrain_clutch_motor_plant.cc \
drivetrain/polydrivetrain_cim_plant.h \
drivetrain/polydrivetrain_cim_plant.cc