Modify third robot code at Madera.
This is the code that was running on 9971 by the end of the
copetition. It's a little kludgy, (mainly the work-arounds
for not having a gyro board), but I decided to commit it
anyway.
There were many changes, but the most important were
probably these:
- Get autonomous to actually work.
- Trick the control loop into giving the shooter 12 volts after
every shot in order to spin it back up.
- Make sure it either powers the shooter or the drivetrain
and not both, so that the shooter wheel can't run so slowly that
frisbees get stuck.
diff --git a/bot3/control_loops/drivetrain/drivetrain.cc b/bot3/control_loops/drivetrain/drivetrain.cc
index b0cb8b4..ca9db92 100644
--- a/bot3/control_loops/drivetrain/drivetrain.cc
+++ b/bot3/control_loops/drivetrain/drivetrain.cc
@@ -22,71 +22,6 @@
// Width of the robot.
const double width = 22.0 / 100.0 * 2.54;
-class DrivetrainMotorsSS {
- public:
- DrivetrainMotorsSS ()
- : loop_(new StateFeedbackLoop<4, 2, 2>(MakeDrivetrainLoop())) {
- _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;
- }
- 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;
- loop_->Y << left, right;
- }
- 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;
- SetRawPosition(left, right);
- LOG(DEBUG, "Left %f->%f Right %f->%f Gyro %f aerror %f ioff %f\n", left + _offset, _left_goal, right - _offset, _right_goal, gyro, angle_error, _integral_offset);
- }
-
- void Update(bool update_observer, bool stop_motors) {
- loop_->Update(update_observer, stop_motors);
- }
-
- void SendMotors(Drivetrain::Output *output) {
- if (output) {
- output->left_voltage = loop_->U(0, 0);
- output->right_voltage = loop_->U(1, 0);
- }
- }
- void PrintMotors() const {
- // LOG(DEBUG, "Left Power %f Right Power %f lg %f rg %f le %f re %f gyro %f\n", U[0], U[1], R[0], R[2], Y[0], Y[1], _gyro);
- ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
- LOG(DEBUG, "E[0, 0]: %f E[1, 0] %f E[2, 0] %f E[3, 0] %f\n", E(0, 0), E(1, 0), E(2, 0), E(3, 0));
- }
-
- 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;
-};
-
class DrivetrainMotorsOL {
public:
DrivetrainMotorsOL() {
@@ -190,6 +125,7 @@
overPower = 0.0;
angular_power = ::std::abs(_throttle) * wheel * sensitivity;
}
+ LOG(DEBUG, "Angular power: %f\n", angular_power);
_right_pwm = _left_pwm = linear_power;
_left_pwm += angular_power;
@@ -236,49 +172,20 @@
};
void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
- const Drivetrain::Position *position,
+ const Drivetrain::Position * /*position*/,
Drivetrain::Output *output,
Drivetrain::Status * /*status*/) {
// TODO(aschuh): These should be members of the class.
- static DrivetrainMotorsSS dt_closedloop;
static DrivetrainMotorsOL dt_openloop;
- bool bad_pos = false;
- if (position == NULL) {
- LOG(WARNING, "no position\n");
- bad_pos = true;
- }
-
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;
-
- dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal,
- right_goal, goal->right_velocity_goal);
- if (!bad_pos) {
- const double left_encoder = position->left_encoder;
- const double right_encoder = position->right_encoder;
- if (gyro.FetchLatest()) {
- dt_closedloop.SetPosition(left_encoder, right_encoder,
- gyro->angle, control_loop_driving);
- } else {
- dt_closedloop.SetRawPosition(left_encoder, right_encoder);
- }
- }
- dt_closedloop.Update(position, output == NULL);
- //dt_closedloop.PrintMotors();
dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
- if (control_loop_driving) {
- dt_closedloop.SendMotors(output);
- } else {
- dt_openloop.SendMotors(output);
- }
+ dt_openloop.SendMotors(output);
}
} // namespace control_loops
diff --git a/bot3/control_loops/drivetrain/drivetrain.h b/bot3/control_loops/drivetrain/drivetrain.h
index 03b3bd0..40bddf7 100644
--- a/bot3/control_loops/drivetrain/drivetrain.h
+++ b/bot3/control_loops/drivetrain/drivetrain.h
@@ -8,13 +8,13 @@
namespace control_loops {
class DrivetrainLoop
- : public aos::control_loops::ControlLoop<control_loops::Drivetrain> {
+ : public aos::control_loops::ControlLoop<control_loops::Drivetrain, false> {
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(
- control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
- : aos::control_loops::ControlLoop<control_loops::Drivetrain>(
+ control_loops::Drivetrain *my_drivetrain = &::bot3::control_loops::drivetrain)
+ : aos::control_loops::ControlLoop<control_loops::Drivetrain, false>(
my_drivetrain) {}
protected: