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/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
index 25bfc74..f32fddd 100644
--- a/bot3/control_loops/shooter/shooter.cc
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -10,7 +10,7 @@
namespace control_loops {
ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
- : aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
+ : aos::control_loops::ControlLoop<control_loops::ShooterLoop, true, false, true>(my_shooter),
loop_(new StateFeedbackLoop<1, 1, 1>(MakeShooterLoop())),
last_velocity_goal_(0) {
loop_->Reset();
@@ -26,9 +26,15 @@
control_loops::ShooterLoop::Status *status) {
double velocity_goal = goal->velocity;
// Our position here is actually a velocity.
+ LOG(DEBUG, "Position is null: %d\n", position == NULL);
average_velocity_ =
(position == NULL ? loop_->X_hat(0, 0) : position->velocity);
double output_voltage = 0.0;
+ bool full_power = false;
+ if (!average_velocity_ && velocity_goal) {
+ LOG(DEBUG, "Giving full power.\n");
+ full_power = true;
+ }
/* if (index_loop.status.FetchLatest() || index_loop.status.get()) {
if (index_loop.status->is_shooting) {
@@ -70,6 +76,10 @@
status->ready = false;
}
LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
+
+ if (full_power) {
+ output_voltage = 12.0;
+ }
if (output) {
output->voltage = output_voltage;
diff --git a/bot3/control_loops/shooter/shooter.h b/bot3/control_loops/shooter/shooter.h
index b9514c3..ec1b81f 100644
--- a/bot3/control_loops/shooter/shooter.h
+++ b/bot3/control_loops/shooter/shooter.h
@@ -12,7 +12,7 @@
namespace control_loops {
class ShooterMotor
- : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
+ : public aos::control_loops::ControlLoop<control_loops::ShooterLoop, true, false, true> {
public:
explicit ShooterMotor(
control_loops::ShooterLoop *my_shooter = &control_loops::shooter);