Improved shooter usability and tested on robot.
Tweaked shooter constants (probably insignificant), linearized victors, added more buttons to allow drivers to run the shooter at several options of speed, and tested this on the robot.
diff --git a/bot3/control_loops/python/shooter.py b/bot3/control_loops/python/shooter.py
index 6efecad..60b3219 100755
--- a/bot3/control_loops/python/shooter.py
+++ b/bot3/control_loops/python/shooter.py
@@ -94,11 +94,11 @@
shooter = Shooter()
close_loop_x = []
close_loop_U = []
- velocity_goal = 300
+ velocity_goal = 350
R = numpy.matrix([[0.0], [velocity_goal]])
- for _ in pylab.linspace(0,1.99,200):
+ for _ in pylab.linspace(0,3.99,400):
# Iterate the position up.
- R = numpy.matrix([[R[0, 0] + 10.5], [velocity_goal]])
+ R = numpy.matrix([[R[0, 0] + velocity_goal / 100], [velocity_goal]])
# Prevents the position goal from going beyond what is necessary.
velocity_weight_scalar = 0.35
max_reference = (
@@ -118,11 +118,12 @@
shooter.Update(U)
close_loop_x.append(shooter.X[1, 0])
close_loop_U.append(U[0, 0] * 10)
+ print U[0, 0]
#pylab.plotfile("shooter.csv", (0,1))
- pylab.plot(pylab.linspace(0,1.99,200), close_loop_U)
+ pylab.plot(pylab.linspace(0,3.99,400), close_loop_U)
#pylab.plotfile("shooter.csv", (0,2))
- pylab.plot(pylab.linspace(0,1.99,200), close_loop_x)
+ pylab.plot(pylab.linspace(0,3.99,400), close_loop_x)
pylab.show()
# Simulate spin down.
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
index 5dd32db..c71b96a 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.cc
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -27,7 +27,7 @@
Eigen::Matrix<double, 2, 1> L;
L << 1.08905775674, 29.7111780621;
Eigen::Matrix<double, 1, 2> K;
- K << 1.42534042426, 0.758151303088;
+ K << 130.450278144, 2.19032372689;
return StateFeedbackController<2, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
@@ -44,4 +44,4 @@
}
} // namespace control_loops
-} // namespace frc971
+} // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
index 2207ab2..fa2237d 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.h
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -15,6 +15,6 @@
StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
} // namespace control_loops
-} // namespace frc971
+} // namespace bot3
-#endif // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#endif // BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
index 6922e1e..199d1e6 100644
--- a/bot3/input/joystick_reader.cc
+++ b/bot3/input/joystick_reader.cc
@@ -134,10 +134,28 @@
push = true;
}
if (data.IsPressed(kFire)) {
+ velocity = 500;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 1))) {
+ velocity = 50;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 2))) {
velocity = 250;
}
+ else if (data.IsPressed(ButtonLocation(3, 5))) {
+ velocity = 300;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 7))) {
+ velocity = 350;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 8))) {
+ velocity = 400;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 10))) {
+ velocity = 450;
+ }
if (data.IsPressed(kIntake)) {
- intake = 0.9;
+ intake = 0.8;
}
shooter.goal.MakeWithBuilder().intake(intake).velocity(velocity).push(push).Send();
#if 0
diff --git a/bot3/output/atom_motor_writer.cc b/bot3/output/atom_motor_writer.cc
index 5ac1bd2..a16dfc4 100644
--- a/bot3/output/atom_motor_writer.cc
+++ b/bot3/output/atom_motor_writer.cc
@@ -47,7 +47,7 @@
shooter.output.FetchLatest();
if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(1, shooter.output->voltage / 12.0, kVictorBounds);
+ SetPWMOutput(1, LinearizeVictor(shooter.output->voltage / 12.0), kVictorBounds);
SetPWMOutput(7, 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);
@@ -58,6 +58,72 @@
}
// TODO(danielp): Add stuff for intake and shooter.
}
+
+ // This linearizes the value from the victor.
+ // Copied form the 2012 code.
+ double LinearizeVictor(double goal_speed) {
+ // These values were derived by putting the robot up on blocks, and driving it
+ // at various speeds. The power required to drive at these speeds was then
+ // recorded and fit with gnuplot.
+ const double deadband_value = 0.082;
+ // If we are outside the range such that the motor is actually moving,
+ // subtract off the constant offset from the deadband. This makes the
+ // function odd and intersect the origin, making the fitting easier to do.
+ if (goal_speed > deadband_value) {
+ goal_speed -= deadband_value;
+ } else if (goal_speed < -deadband_value) {
+ goal_speed += deadband_value;
+ } else {
+ goal_speed = 0.0;
+ }
+ goal_speed = goal_speed / (1.0 - deadband_value);
+
+ double goal_speed2 = goal_speed * goal_speed;
+ double goal_speed3 = goal_speed2 * goal_speed;
+ double goal_speed4 = goal_speed3 * goal_speed;
+ double goal_speed5 = goal_speed4 * goal_speed;
+ double goal_speed6 = goal_speed5 * goal_speed;
+ double goal_speed7 = goal_speed6 * goal_speed;
+
+ // Constants for the 5th order polynomial
+ double victor_fit_e1 = 0.437239;
+ double victor_fit_c1 = -1.56847;
+ double victor_fit_a1 = (- (125.0 * victor_fit_e1 + 125.0
+ * victor_fit_c1 - 116.0) / 125.0);
+ double answer_5th_order = (victor_fit_a1 * goal_speed5
+ + victor_fit_c1 * goal_speed3
+ + victor_fit_e1 * goal_speed);
+
+ // Constants for the 7th order polynomial
+ double victor_fit_c2 = -5.46889;
+ double victor_fit_e2 = 2.24214;
+ double victor_fit_g2 = -0.042375;
+ double victor_fit_a2 = (- (125.0 * (victor_fit_c2 + victor_fit_e2
+ + victor_fit_g2) - 116.0) / 125.0);
+ double answer_7th_order = (victor_fit_a2 * goal_speed7
+ + victor_fit_c2 * goal_speed5
+ + victor_fit_e2 * goal_speed3
+ + victor_fit_g2 * goal_speed);
+
+
+ // Average the 5th and 7th order polynomials, and add a bit of linear power in
+ // as well. The average turns out to nicely fit the data in gnuplot with nice
+ // smooth curves, and the linear power gives it a bit more punch at low speeds
+ // again. Stupid victors.
+ double answer = 0.85 * 0.5 * (answer_7th_order + answer_5th_order)
+ + .15 * goal_speed * (1.0 - deadband_value);
+
+ // Add the deadband power back in to make it so that the motor starts moving
+ // when any power is applied. This is what the fitting assumes.
+ if (answer > 0.001) {
+ answer += deadband_value;
+ } else if (answer < -0.001) {
+ answer -= deadband_value;
+ }
+
+ return answer;
+ }
+
};
} // namespace output