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/autonomous/auto.cc b/bot3/autonomous/auto.cc
index 8a3c1ad..89f89a0 100644
--- a/bot3/autonomous/auto.cc
+++ b/bot3/autonomous/auto.cc
@@ -7,7 +7,6 @@
#include "aos/common/logging/logging.h"
#include "bot3/autonomous/auto.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "bot3/control_loops/shooter/shooter_motor.q.h"
#include "frc971/constants.h"
@@ -27,28 +26,37 @@
void SetShooter(double velocity, bool push) {
LOG(INFO, "Setting shooter velocity to %f\n", velocity);
- control_loops::shooter.goal.MakeWithBuilder()
+ control_loops::shooter.goal.MakeWithBuilder().intake(0)
.velocity(velocity).push(push).Send();
}
-bool ShooterReady() {
- control_loops::shooter.status.FetchLatest();
- return control_loops::shooter.status->ready;
+void SpinUp() {
+ LOG(INFO, "Tricking shooter into running at full power...\n");
+ control_loops::shooter.position.MakeWithBuilder().velocity(0).Send();
}
-// start with N discs in the indexer
+bool ShooterReady() {
+ bool ready = control_loops::shooter.status.FetchNextBlocking() && control_loops::shooter.status->ready;
+ LOG(DEBUG, "Shooter ready: %d\n", ready);
+ return ready;
+}
+
void HandleAuto() {
- double shooter_velocity = 250.0;
+ double shooter_velocity = 500.0;
+ bool first_accl = true;
while (!ShouldExitAuto()) {
SetShooter(shooter_velocity, false);
- while (!ShouldExitAuto() && !ShooterReady());
+ if (first_accl) {
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(3.0));
+ first_accl = false;
+ }
if (ShouldExitAuto()) return;
SetShooter(shooter_velocity, true);
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.5));
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.25));
SetShooter(shooter_velocity, false);
- // Waits because, until we have feedback,
- // the shooter will always think it's ready.
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.5));
+ // We just shot, trick it into spinning back up.
+ SpinUp();
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(2.0));
}
return;
}
diff --git a/bot3/autonomous/autonomous.gyp b/bot3/autonomous/autonomous.gyp
index 54b0816..8835e84 100644
--- a/bot3/autonomous/autonomous.gyp
+++ b/bot3/autonomous/autonomous.gyp
@@ -25,9 +25,7 @@
'auto_queue',
'<(AOS)/common/common.gyp:controls',
'<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
'<(AOS)/common/util/util.gyp:trapezoid_profile',