actually use the gyro and do auto in high gear
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 092f66e..c92b45e 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -26,43 +26,44 @@
namespace control_loops {
// Width of the robot.
-const double width = 22.0 / 100.0 * 2.54;
+const double width = 25.0 / 100.0 * 2.54;
class DrivetrainMotorsSS {
public:
DrivetrainMotorsSS()
: loop_(new StateFeedbackLoop<4, 2, 2>(
- constants::GetValues().make_drivetrain_loop())) {
- _offset = 0;
- _integral_offset = 0;
- _left_goal = 0.0;
- _right_goal = 0.0;
- _raw_left = 0.0;
- _raw_right = 0.0;
- _control_loop_driving = false;
+ 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),
+ control_loop_driving_(false) {
+ // 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;
+
+ void SetGoal(double left, double left_velocity, double right,
+ double right_velocity) {
+ left_goal_ = left;
+ right_goal_ = right;
loop_->R << left, left_velocity, right, right_velocity;
}
void SetRawPosition(double left, double right) {
- _raw_right = right;
- _raw_left = left;
+ raw_right_ = right;
+ raw_left_ = left;
Eigen::Matrix<double, 2, 1> Y;
- Y << left, right;
+ Y << left + filtered_offset_, right - filtered_offset_;
loop_->Correct(Y);
}
void SetPosition(
double left, double right, double gyro, bool control_loop_driving) {
// Decay the offset quickly because this gyro is great.
- _offset = (0.25) * (right - left - gyro * width) / 2.0 + 0.75 * _offset;
- //const double angle_error = (_right_goal - _left_goal) / width - (_raw_right - _offset - _raw_left - _offset) / width;
- // TODO(aschuh): Add in the gyro.
- _integral_offset = 0.0;
- _offset = 0.0;
- _gyro = gyro;
- _control_loop_driving = control_loop_driving;
+ const double offset = (right - left - gyro * width) / 2.0;
+ filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
+ gyro_ = gyro;
+ control_loop_driving_ = control_loop_driving;
SetRawPosition(left, right);
}
@@ -71,7 +72,7 @@
}
void Update(bool stop_motors) {
- if (_control_loop_driving) {
+ if (control_loop_driving_) {
loop_->Update(stop_motors);
} else {
if (stop_motors) {
@@ -102,20 +103,21 @@
if (output) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
+ output->left_high = true;
+ output->right_high = true;
}
}
private:
::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> loop_;
- double _integral_offset;
- double _offset;
- double _gyro;
- double _left_goal;
- double _right_goal;
- double _raw_left;
- double _raw_right;
- bool _control_loop_driving;
+ double filtered_offset_;
+ double gyro_;
+ double left_goal_;
+ double right_goal_;
+ double raw_left_;
+ double raw_right_;
+ bool control_loop_driving_;
};
class PolyDrivetrain {
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index 231eefb..b254fd7 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -7,7 +7,7 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
A << 1.0, 0.00860955515291, 0.0, 0.000228184998733, 0.0, 0.735841675858, 0.0, 0.0410810558113, 0.0, 0.000228184998733, 1.0, 0.00860955515291, 0.0, 0.0410810558113, 0.0, 0.735841675858;
Eigen::Matrix<double, 4, 2> B;
@@ -23,23 +23,101 @@
return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<4, 2, 2> MakeDrivetrainController() {
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00860667098456, 0.0, 7.04111872002e-05, 0.0, 0.735048848179, 0.0, 0.0131811893199, 0.0, 0.000245343870066, 1.0, 0.00957169266049, 0.0, 0.045929121897, 0.0, 0.915703853642;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000272809358971, -2.57343985847e-05, 0.0518765869984, -0.00481755802263, -4.80375440247e-05, 0.00015654091672, -0.00899277497558, 0.0308091755839;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 0, 0, 0, 0, 1, 0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> U_max;
+ U_max << 12.0, 12.0;
+ Eigen::Matrix<double, 2, 1> U_min;
+ U_min << -12.0, -12.0;
+ return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00957169266049, 0.0, 0.000245343870066, 0.0, 0.915703853642, 0.0, 0.045929121897, 0.0, 7.04111872002e-05, 1.0, 0.00860667098456, 0.0, 0.0131811893199, 0.0, 0.735048848179;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.00015654091672, -4.80375440247e-05, 0.0308091755839, -0.00899277497558, -2.57343985847e-05, 0.000272809358971, -0.00481755802263, 0.0518765869984;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 0, 0, 0, 0, 1, 0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> U_max;
+ U_max << 12.0, 12.0;
+ Eigen::Matrix<double, 2, 1> U_min;
+ U_min << -12.0, -12.0;
+ return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00957076892085, 0.0, 7.56192087769e-05, 0.0, 0.915439806567, 0.0, 0.0146814193986, 0.0, 7.56192087769e-05, 1.0, 0.00957076892085, 0.0, 0.0146814193986, 0.0, 0.915439806567;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 0.000156878531877, -2.76378646165e-05, 0.0309056814511, -0.00536587314624, -2.76378646165e-05, 0.000156878531877, -0.00536587314624, 0.0309056814511;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 0, 0, 0, 0, 1, 0;
+ Eigen::Matrix<double, 2, 2> D;
+ D << 0, 0, 0, 0;
+ Eigen::Matrix<double, 2, 1> U_max;
+ U_max << 12.0, 12.0;
+ Eigen::Matrix<double, 2, 1> U_min;
+ U_min << -12.0, -12.0;
+ return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.57584167586, 0.0410810558113, 50.0130674801, 4.93325200717, 0.0410810558113, 1.57584167586, 4.93325200717, 50.0130674801;
+ L << 1.46584167586, 0.0410810558113, 41.9788073691, 4.62131736499, 0.0410810558113, 1.46584167586, 4.62131736499, 41.9788073691;
Eigen::Matrix<double, 2, 4> K;
- K << 128.210620632, 6.93828382074, 5.11036686771, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
+ K << 128.210620632, 6.93828382074, 5.1103668677, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowLowPlantCoefficients());
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.45856045663, 0.0145216082167, 41.3651523316, 1.69047524315, 0.0145216082167, 1.65219224519, 3.02153581754, 64.1990849762;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 127.841025245, 6.90618982868, -2.11442482189, 0.171361719101, 11.257083857, 1.47190974842, 138.457761234, 11.0770574926;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowHighPlantCoefficients());
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.64511269416, 0.045918446918, 63.7288738736, 5.55411342044, 0.045918446918, 1.46564000767, 3.81771765442, 41.8737588606;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 138.457761234, 11.0770574926, 11.257083857, 1.47190974842, -2.11442482191, 0.171361719101, 127.841025245, 6.90618982869;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighLowPlantCoefficients());
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 1.64543980657, 0.0146814193986, 63.6243330534, 1.89166687195, 0.0146814193986, 1.64543980657, 1.89166687195, 63.6243330534;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 138.52410152, 11.0779399816, 3.96842371774, 0.882728086516, 3.96842371774, 0.882728086517, 138.52410152, 11.0779399816;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighHighPlantCoefficients());
}
StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainPlantCoefficients());
+ ::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);
}
StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
- ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainController());
+ ::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);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
index 3829e9a..366f95d 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -6,9 +6,21 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients();
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients();
-StateFeedbackController<4, 2, 2> MakeDrivetrainController();
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController();
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController();
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController();
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController();
StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 3d6e441..910196e 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -51,8 +51,8 @@
class Drivetrain(control_loop.ControlLoop):
- def __init__(self, left_low=True, right_low=True):
- super(Drivetrain, self).__init__("Drivetrain")
+ def __init__(self, name="Drivetrain", left_low=True, right_low=True):
+ super(Drivetrain, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 2.42
# Stall Current in Amps
@@ -143,8 +143,8 @@
print self.K
print numpy.linalg.eig(self.A - self.B * self.K)[0]
- self.hlp = 0.07
- self.llp = 0.09
+ self.hlp = 0.12
+ self.llp = 0.15
self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
self.U_max = numpy.matrix([[12.0], [12.0]])
@@ -218,12 +218,17 @@
# Write the generated constants out to a file.
print "Output one"
- drivetrain = Drivetrain()
+ drivetrain_low_low = Drivetrain(name="DrivetrainLowLow", left_low=True, right_low=True)
+ drivetrain_low_high = Drivetrain(name="DrivetrainLowHigh", left_low=True, right_low=False)
+ 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) != 5:
print "Expected .h file name and .cc file name"
else:
- dog_loop_writer = control_loop.ControlLoopWriter("Drivetrain", [drivetrain])
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
+ drivetrain_high_low, drivetrain_high_high])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
diff --git a/frc971/queues/other_sensors.q b/frc971/queues/other_sensors.q
index 7181ce9..e986a10 100644
--- a/frc971/queues/other_sensors.q
+++ b/frc971/queues/other_sensors.q
@@ -7,6 +7,8 @@
queue OtherSensors other_sensors;
message GyroReading {
+ // Positive is counter-clockwise (Austin says "it's Positive").
+ // Right-hand coordinate system around the Z-axis going up.
double angle;
};
queue GyroReading gyro_reading;