Retune finisher to not oscillate
There is a small overshoot which appears to be battery or falcon
related, but everything looks a lot smoother and in phase. I don't see
the wild oscillations we were seeing before.
The plus side is that this appears to have made the ball counter work a
lot better.
Change-Id: Id43da39b12ace9516d3698f41ecc667afdab6de8
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 7ccccd6..8c33cc3 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -20,7 +20,7 @@
G = 44.0 / 40.0
# Overall flywheel inertia.
J = 0.00507464
-J = 0.008
+J = 0.0035
def AddResistance(motor, resistance):
motor.resistance += resistance
@@ -40,7 +40,7 @@
q_vel=10.0,
q_voltage=4.0,
r_pos=0.01,
- controller_poles=[.89])
+ controller_poles=[.93])
def main(argv):
@@ -58,4 +58,5 @@
if __name__ == '__main__':
argv = FLAGS(sys.argv)
+ glog.init()
sys.exit(main(argv))
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index caaee96..6f41a17 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -70,6 +70,10 @@
self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ glog.debug('K: %s', str(self.K))
+ glog.debug('Poles: %s',
+ str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
class Flywheel(VelocityFlywheel):
def __init__(self, params, name="Flywheel"):
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 99854fe..9be6273 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -116,6 +116,7 @@
// come back up close to the last local maximum or is greater than it, the
// ball has been shot.
balls_shot_++;
+ VLOG(1) << "Shot ball at " << position_timestamp;
ball_in_finisher_ = false;
} else if (!ball_in_finisher_ &&
(finisher_goal() > kVelocityToleranceFinisher)) {
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index c154c3d..0e51b7b 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -1087,18 +1087,21 @@
ball_in_finisher_ = false;
finisher_velocity_below_goal = false;
balls_shot++;
+ LOG(INFO) << "Shot a ball at " << test_event_loop_->monotonic_now();
}
// Since here we are calculating the dip from the goal instead of the
// local maximum, the shooter could have calculated that a ball was shot
// slightly before we did if the local maximum was slightly below the
// goal.
- EXPECT_TRUE((superstructure_status_fetcher_->shooter()->balls_shot() ==
- balls_shot) ||
- ((superstructure_status_fetcher_->shooter()->balls_shot() ==
- balls_shot + 1) &&
- (finisher_velocity_dip -
- shooter::Shooter::kVelocityToleranceFinisher <
- 0.2)));
+ if (superstructure_status_fetcher_->shooter()->balls_shot() !=
+ balls_shot) {
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->balls_shot(),
+ balls_shot + 1)
+ << ": Failed at " << test_event_loop_->monotonic_now();
+ EXPECT_LT(finisher_velocity_dip,
+ 0.2 + shooter::Shooter::kVelocityToleranceFinisher)
+ << ": Failed at " << test_event_loop_->monotonic_now();
+ }
},
dt());
@@ -1108,8 +1111,8 @@
// Maximum (since it is negative) flywheel voltage offsets for simulating the
// friction of a ball at different finisher speeds.
// Slower speeds require a higher magnitude of voltage offset.
- static constexpr double kFastSpeedVoltageOffsetWithBall = -4.1;
- static constexpr double kSlowSpeedVoltageOffsetWithBall = -4.5;
+ static constexpr double kFastSpeedVoltageOffsetWithBall = -3.1;
+ static constexpr double kSlowSpeedVoltageOffsetWithBall = -3.5;
SetFinisherGoalAfter(kFastShootingSpeed, chrono::seconds(1));
// Simulate shooting balls by applying friction to the finisher
@@ -1131,7 +1134,7 @@
ApplyFrictionToFinisherAfter(kSlowSpeedVoltageOffsetWithBall, true,
chrono::seconds(31));
// This smaller decrease in velocity shouldn't be counted as a ball
- ApplyFrictionToFinisherAfter(kSlowSpeedVoltageOffsetWithBall / 2, false,
+ ApplyFrictionToFinisherAfter(kSlowSpeedVoltageOffsetWithBall / 10.0, false,
chrono::seconds(34));
SetFinisherGoalAfter(kFastShootingSpeed, chrono::seconds(38));
@@ -1142,7 +1145,7 @@
// This slow positive voltage offset that speeds up the flywheel instead of
// slowing it down shouldn't be counted as a ball.
// We wouldn't expect a positive voltage offset of more than ~2 volts.
- ApplyFrictionToFinisherAfter(2, false, chrono::seconds(47));
+ ApplyFrictionToFinisherAfter(1, false, chrono::seconds(47));
RunFor(chrono::seconds(50));