got SetGoalPosition and apply_some_voltage implemented.
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index dfc440c..86991e8 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -79,8 +79,12 @@
double real_position = position->position - calibration_position_;
+ // don't even let the control loops run
bool shooter_loop_disable = false;
+ // adds voltage to take up slack in gears before shot
+ bool apply_some_voltage = false;
+
switch (state_) {
case STATE_INITIALIZE:
// start off with the assumption that we are at the value
@@ -209,7 +213,7 @@
if (position->plunger_back_hall_effect) {
prepare_fire_end_time_ = Time::Now(Time::kDefaultClock)
+ Time::InMS(40.0);
- shooter_.ApplySomeVoltage();
+ apply_some_voltage = true;
state_ = STATE_PREPARE_FIRE;
} else {
state_ = STATE_REQUEST_LOAD;
@@ -218,7 +222,7 @@
case STATE_PREPARE_FIRE:
shooter_loop_disable = true;
if (Time::Now(Time::kDefaultClock) < prepare_fire_end_time_) {
- shooter_.ApplySomeVoltage();
+ apply_some_voltage = true;
} else {
state_ = STATE_FIRE;
cycles_not_moved_ = 0;
@@ -287,9 +291,14 @@
real_position, position->position);
}
- if (!shooter_loop_disable) {
+ if (apply_some_voltage) {
+ output->voltage = 2.0;
+ } else if (!shooter_loop_disable) {
output->voltage = shooter_.voltage();
+ } else {
+ output->voltage = 0.0;
}
+
status->done = ::std::abs(position->position - PowerToPosition(goal->shot_power)) < 0.004;
}
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index c19328f..6780712 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -103,16 +103,7 @@
// austin said something about which matrix to set, but I didn't under
// very much of it
//some_matrix = {desired_position, desired_velocity};
- printf("%s:%d : seg fault (%.2f, %.2f)\n", __FILE__, __LINE__,
- desired_position, desired_velocity);
- *(const char **)(NULL) = "seg fault";
- }
-
- // apply a small amout of voltage outside the loop so we can
- // take up backlash in gears
- void ApplySomeVoltage() {
- printf("%s:%d : seg fault\n", __FILE__, __LINE__);
- *(const char **)(NULL) = "seg fault";
+ R << desired_position, desired_velocity, 0;
}
double position() const { return X_hat(0, 0); }