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/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index 5d22300..e397cef 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -9,16 +9,17 @@
// TODO(aschuh): Tests.
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::ZeroOutputs() {
+template <class T, bool has_position, bool fail_no_position, bool ignore_stale>
+void ControlLoop<T, has_position, fail_no_position, ignore_stale>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::Iterate() {
+template <class T, bool has_position, bool fail_no_position, bool ignore_stale>
+void ControlLoop<T, has_position, fail_no_position, ignore_stale>::Iterate() {
+ bool use_model = false;
// Temporary storage for printing out inputs and outputs.
char state[1024];
@@ -48,21 +49,26 @@
if (control_loop_->position.FetchLatest()) {
position = control_loop_->position.get();
} else {
+ if (ignore_stale) {
+ use_model = true;
+ }
if (control_loop_->position.get()) {
int msec_age = control_loop_->position.Age().ToMSec();
if (!control_loop_->position.IsNewerThanMS(kPositionTimeoutMs)) {
LOG(ERROR, "Stale position. %d ms > %d ms. Outputs disabled.\n",
msec_age, kPositionTimeoutMs);
- //ZeroOutputs();
- //eturn;
+ if (!ignore_stale) {
+ ZeroOutputs();
+ return;
+ }
} else {
LOG(ERROR, "Stale position. %d ms\n", msec_age);
}
} else {
LOG(ERROR, "Never had a position.\n");
if (fail_no_position) {
- // ZeroOutputs();
- // return;
+ ZeroOutputs();
+ return;
}
}
}
@@ -95,7 +101,11 @@
return;
}
+ LOG(DEBUG, "Outputs enabled: %d\n", outputs_enabled);
if (outputs_enabled) {
+ if (use_model) {
+ position = NULL;
+ }
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
RunIteration(goal, position, output.get(), status.get());
@@ -114,8 +124,8 @@
status.Send();
}
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::Run() {
+template <class T, bool has_position, bool fail_no_position, bool ignore_stale>
+void ControlLoop<T, has_position, fail_no_position, ignore_stale>::Run() {
while (true) {
time::SleepUntil(NextLoopTime());
Iterate();
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index 6af7235..753008c 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -50,7 +50,7 @@
// If has_position is false, the control loop will always use NULL as the
// position and not check the queue. This is used for "loops" that control
// motors open loop.
-template <class T, bool has_position = true, bool fail_no_position = true>
+template <class T, bool has_position = true, bool fail_no_position = true, bool ignore_stale = false>
class ControlLoop : public SerializableControlLoop {
public:
// Maximum age of position packets before the loop will be disabled due to
diff --git a/bot3/atom_code/scripts/start_list.txt b/bot3/atom_code/scripts/start_list.txt
index f8a0ba7..ccd2dfc 100644
--- a/bot3/atom_code/scripts/start_list.txt
+++ b/bot3/atom_code/scripts/start_list.txt
@@ -4,4 +4,3 @@
drivetrain
auto
shooter
-gyro_sensor_receiver
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',
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:
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);
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
index 199d1e6..dbfd92a 100644
--- a/bot3/input/joystick_reader.cc
+++ b/bot3/input/joystick_reader.cc
@@ -40,10 +40,12 @@
const ButtonLocation kIntake(3, 4);
class Reader : public ::aos::input::JoystickInput {
+ bool last_push_;
+ bool shooting_;
public:
static const bool kWristAlwaysDown = false;
- Reader() {
+ Reader() : shooting_(false) {
printf("\nRunning Bot3 JoystickReader!\n");
shifters.MakeWithBuilder().set(true).Send();
}
@@ -60,6 +62,8 @@
LOG(INFO, "Stopping auto mode\n");
::bot3::autonomous::autonomous.MakeWithBuilder()
.run_auto(false).Send();
+ } else {
+ LOG(DEBUG, "Running auto\n");
}
} else { // teleop
bool is_control_loop_driving = false;
@@ -67,54 +71,18 @@
double right_goal = 0.0;
const double wheel = data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
- LOG(DEBUG, "wheel %f throttle %f\n", wheel, throttle);
+ const bool quickturn = data.IsPressed(kQuickTurn);
+ LOG(DEBUG, "wheel %f throttle %f quickturn %d\n", wheel, throttle, quickturn);
//const double kThrottleGain = 1.0 / 2.5;
- if (data.IsPressed(kDriveControlLoopEnable1) ||
- data.IsPressed(kDriveControlLoopEnable2)) {
- LOG(INFO, "Control loop driving is currently not supported by this robot.\n");
-#if 0
- static double distance = 0.0;
- static double angle = 0.0;
- static double filtered_goal_distance = 0.0;
- if (data.PosEdge(kDriveControlLoopEnable1) ||
- data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
- distance = (drivetrain.position->left_encoder +
- drivetrain.position->right_encoder) / 2.0
- - throttle * kThrottleGain / 2.0;
- angle = gyro->angle;
- filtered_goal_distance = distance;
- }
+ if (!shooting_) {
+ if (!drivetrain.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear).quickturn(quickturn)
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal).right_goal(right_goal).Send()) {
+ LOG(WARNING, "sending stick values failed\n");
}
- is_control_loop_driving = true;
-
- //const double gyro_angle = Gyro.View().angle;
- const double goal_theta = angle - wheel * 0.27;
- const double goal_distance = distance + throttle * kThrottleGain;
- const double robot_width = 22.0 / 100.0 * 2.54;
- const double kMaxVelocity = 0.6;
- if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
- filtered_goal_distance += kMaxVelocity * 0.02;
- } else if (goal_distance < -kMaxVelocity * 0.02 +
- filtered_goal_distance) {
- filtered_goal_distance -= kMaxVelocity * 0.02;
- } else {
- filtered_goal_distance = goal_distance;
- }
- left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
- right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
- is_high_gear = false;
-
- LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
-#endif
- }
- if (!(drivetrain.goal.MakeWithBuilder()
- .steering(wheel)
- .throttle(throttle)
- .highgear(is_high_gear).quickturn(data.IsPressed(kQuickTurn))
- .control_loop_driving(is_control_loop_driving)
- .left_goal(left_goal).right_goal(right_goal).Send())) {
- LOG(WARNING, "sending stick values failed\n");
}
if (data.PosEdge(kShiftHigh)) {
@@ -128,16 +96,22 @@
shooter.status.FetchLatest();
bool push = false;
+ bool full_power = false;
double velocity = 0.0;
double intake = 0.0;
if (data.IsPressed(kPush) && shooter.status->ready) {
push = true;
}
+ if (!push && last_push_) {
+ //Falling edge
+ full_power = true;
+ }
+ last_push_ = push;
if (data.IsPressed(kFire)) {
velocity = 500;
}
else if (data.IsPressed(ButtonLocation(3, 1))) {
- velocity = 50;
+ velocity = 100;
}
else if (data.IsPressed(ButtonLocation(3, 2))) {
velocity = 250;
@@ -157,72 +131,20 @@
if (data.IsPressed(kIntake)) {
intake = 0.8;
}
- shooter.goal.MakeWithBuilder().intake(intake).velocity(velocity).push(push).Send();
-#if 0
- ::aos::ScopedMessagePtr<frc971::control_loops::ShooterLoop::Goal> shooter_goal =
- shooter.goal.MakeMessage();
- shooter_goal->velocity = 0;
- if (data.IsPressed(kPitShot1) && data.IsPressed(kPitShot2)) {
- shooter_goal->velocity = 131;
- } else if (data.IsPressed(kLongShot)) {
-#if 0
- target_angle.FetchLatest();
- if (target_angle.IsNewerThanMS(500)) {
- shooter_goal->velocity = target_angle->shooter_speed;
- angle_adjust_goal = target_angle->shooter_angle;
- // TODO(brians): do the math right here
- wrist_up_position = 0.70;
- } else {
- LOG(WARNING, "camera frame too old\n");
- // pretend like no button is pressed
+ if (abs(throttle) < 0.2 && !quickturn) {
+ shooting_ = true;
+ shooter.goal.MakeWithBuilder().intake(intake).velocity(velocity).push(push).Send();
+ if (full_power) {
+ LOG(DEBUG, "Zeroing position.velocity\n");
+ shooter.position.MakeWithBuilder().velocity(0).Send();
}
-#endif
- shooter_goal->velocity = 360;
- } else if (data.IsPressed(kMediumShot)) {
-#if 0
- shooter_goal->velocity = 375;
- wrist_up_position = 0.70;
- angle_adjust_goal = 0.564;
-#endif
- // middle wheel on the back line (same as auto)
- shooter_goal->velocity = 395;
- } else if (data.IsPressed(kShortShot)) {
- shooter_goal->velocity = 375;
- }
-
- //TODO (daniel) Modify this for hopper and shooter.
- ::aos::ScopedMessagePtr<frc971::control_loops::IndexLoop::Goal> index_goal =
- index_loop.goal.MakeMessage();
- if (data.IsPressed(kFire)) {
- // FIRE
- index_goal->goal_state = 4;
- } else if (shooter_goal->velocity != 0) {
- // get ready to shoot
- index_goal->goal_state = 3;
- } else if (data.IsPressed(kIntake)) {
- // intake
- index_goal->goal_state = 2;
} else {
- // get ready to intake
- index_goal->goal_state = 1;
+ shooting_ = false;
}
- index_goal->force_fire = data.IsPressed(kForceFire);
-
- const bool index_up = data.IsPressed(kForceIndexUp);
- const bool index_down = data.IsPressed(kForceIndexDown);
- index_goal->override_index = index_up || index_down;
- if (index_up && index_down) {
- index_goal->index_voltage = 0.0;
- } else if (index_up) {
- index_goal->index_voltage = 12.0;
- } else if (index_down) {
- index_goal->index_voltage = -12.0;
+ if (!velocity) {
+ shooting_ = false;
}
-
- index_goal.Send();
- shooter_goal.Send();
-#endif
- }
+ }
}
};
diff --git a/bot3/output/atom_motor_writer.cc b/bot3/output/atom_motor_writer.cc
index a16dfc4..d5755e4 100644
--- a/bot3/output/atom_motor_writer.cc
+++ b/bot3/output/atom_motor_writer.cc
@@ -47,8 +47,8 @@
shooter.output.FetchLatest();
if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(1, LinearizeVictor(shooter.output->voltage / 12.0), kVictorBounds);
- SetPWMOutput(7, shooter.output->intake, kVictorBounds);
+ SetPWMOutput(8, LinearizeVictor(shooter.output->voltage / 12.0), kVictorBounds);
+ SetPWMOutput(1, shooter.output->intake, kVictorBounds);
SetSolenoid(4, shooter.output->push);
LOG(DEBUG, "shooter: %lf, intake: %lf, push: %d", shooter.output->voltage, shooter.output->intake, shooter.output->push);
} else {