Tune new hood
Now that we've got better hardware, remove all the hacks to make the old
hood work and tune it. Much better!
Change-Id: I923cff6c133a23e732b7b593d92a5fa40398a298
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index f040f5d..2b8c54c 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -84,6 +84,7 @@
"//frc971/wpilib:imu_plotter",
"//y2020/control_loops/superstructure:accelerator_plotter",
"//y2020/control_loops/superstructure:finisher_plotter",
+ "//y2020/control_loops/superstructure:hood_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 812c245..5b7ad96 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -30,6 +30,8 @@
'org_frc971/y2020/control_loops/superstructure/finisher_plotter'
import {plotAccelerator} from
'org_frc971/y2020/control_loops/superstructure/accelerator_plotter'
+import {plotHood} from
+ 'org_frc971/y2020/control_loops/superstructure/hood_plotter'
import {plotDemo} from 'org_frc971/aos/network/www/demo_plot';
import {plotData} from 'org_frc971/frc971/analysis/plot_data_utils';
@@ -90,6 +92,7 @@
['Robot State', new PlotState(plotDiv, plotRobotState)],
['Finisher', new PlotState(plotDiv, plotFinisher)],
['Accelerator', new PlotState(plotDiv, plotAccelerator)],
+ ['Hood', new PlotState(plotDiv, plotHood)],
['C++ Plotter', new PlotState(plotDiv, plotData)],
]);
diff --git a/y2020/constants.cc b/y2020/constants.cc
index d59a167..b3551aa 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -35,7 +35,7 @@
*const hood = &r->hood;
// Hood constants.
- hood->zeroing_voltage = 3.0;
+ hood->zeroing_voltage = 2.0;
hood->operating_voltage = 12.0;
hood->zeroing_profile_params = {0.5, 3.0};
hood->default_profile_params = {6.0, 30.0};
@@ -109,8 +109,6 @@
break;
case kCompTeamNumber:
- hood->zeroing_constants.measured_absolute_position = 0.266691815725476;
-
intake->zeroing_constants.measured_absolute_position =
1.42977866919024 - Values::kIntakeZero();
@@ -119,8 +117,9 @@
turret_params->zeroing_constants.measured_absolute_position =
0.547478339799516;
- hood->zeroing_constants.measured_absolute_position = 0.03207;
- hood->zeroing_constants.single_turn_measured_absolute_position = 0.31055;
+ hood->zeroing_constants.measured_absolute_position = 0.0344482433884915;
+ hood->zeroing_constants.single_turn_measured_absolute_position =
+ 0.31055891442198;
break;
case kPracticeTeamNumber:
diff --git a/y2020/constants.h b/y2020/constants.h
index f7d67e0..de3303b 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -61,10 +61,10 @@
static constexpr ::frc971::constants::Range kHoodRange() {
return ::frc971::constants::Range{
- 0.00, // Back Hard
- 0.64, // Front Hard
- 0.01, // Back Soft
- 0.63 // Front Soft
+ -0.01, // Back Hard
+ 0.65, // Front Hard
+ 0.00, // Back Soft
+ 0.63 // Front Soft
};
}
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index db6b727..eacb7fd 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -21,9 +21,9 @@
# joint. We are currently electing to ignore potential non-linearity.
range_of_travel_radians = (38.0 * numpy.pi / 180.0)
-# 0.472441 inches/turn (12 mm)
+# 0.5 inches/turn
# 6.7725 inches of travel
-turns_of_leadscrew_per_range_of_travel = 6.7725 / 0.472441
+turns_of_leadscrew_per_range_of_travel = 6.7725 / 0.5
radians_per_turn = range_of_travel_radians / turns_of_leadscrew_per_range_of_travel
@@ -33,13 +33,13 @@
name='Hood',
motor=control_loop.BAG(),
G=radians_per_turn_of_motor / (2.0 * numpy.pi),
- J=4.0,
+ J=0.1,
q_pos=0.15,
- q_vel=5.0,
+ q_vel=10.0,
kalman_q_pos=0.12,
kalman_q_vel=10.0,
- kalman_q_voltage=15.0,
- kalman_r_position=0.05)
+ kalman_q_voltage=30.0,
+ kalman_r_position=0.02)
def main(argv):
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 885bf78..7ad8f97 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -156,3 +156,13 @@
"//aos/network/www:proxy",
],
)
+
+ts_library(
+ name = "hood_plotter",
+ srcs = ["hood_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:proxy",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/hood_plotter.ts b/y2020/control_loops/superstructure/hood_plotter.ts
new file mode 100644
index 0000000..6de8883
--- /dev/null
+++ b/y2020/control_loops/superstructure/hood_plotter.ts
@@ -0,0 +1,54 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+const RED = AosPlotter.RED;
+const GREEN = AosPlotter.GREEN;
+const BLUE = AosPlotter.BLUE;
+const BROWN = AosPlotter.BROWN;
+const PINK = AosPlotter.PINK;
+const CYAN = AosPlotter.CYAN;
+const WHITE = AosPlotter.WHITE;
+
+export function plotHood(conn: Connection, element: Element) : void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Status');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ var currentTop = 0;
+
+ // Robot Enabled/Disabled and Mode
+ const positionPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ positionPlot.plot.getAxisLabels().setTitle('Position');
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel('rad');
+ positionPlot.plot.setDefaultYRange([-0.1, 0.7]);
+
+ positionPlot.addMessageLine(goal, ['hood', 'unsafe_goal']).setColor(BLUE).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'goal_position']).setColor(RED).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'position']).setColor(GREEN).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'velocity']).setColor(PINK).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'calculated_velocity']).setColor(BROWN).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'estimator_state', 'position']).setColor(CYAN).setPointSize(0.0);
+
+ const voltagePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Volts');
+ voltagePlot.plot.setDefaultYRange([-4.0, 16.0]);
+
+ voltagePlot.addMessageLine(output, ['hood_voltage']).setColor(BLUE).setPointSize(0.0);
+ voltagePlot.addMessageLine(status, ['hood', 'voltage_error']).setColor(RED).setPointSize(0.0);
+ voltagePlot.addMessageLine(robotState, ['voltage_battery']).setColor(GREEN).setPointSize(0.0);
+}
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 6e48a21..8cdcf8c 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -30,8 +30,10 @@
std::abs(goal->velocity_accelerator() -
accelerator_left_.avg_angular_velocity()) < kVelocityTolerance &&
std::abs(goal->velocity_accelerator() -
- accelerator_right_.avg_angular_velocity()) < kVelocityTolerance &&
- std::abs(goal->velocity_finisher() - finisher_.velocity()) < kVelocityTolerance &&
+ accelerator_right_.avg_angular_velocity()) <
+ kVelocityTolerance &&
+ std::abs(goal->velocity_finisher() - finisher_.velocity()) <
+ kVelocityTolerance &&
std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
kVelocityTolerance &&
std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index e6c1375..c408f67 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -139,17 +139,6 @@
output_struct.turret_voltage =
std::clamp(output_struct.turret_voltage, -turret_.operating_voltage(),
turret_.operating_voltage());
-
- // Friction is a pain and putting a really high burden on the integrator.
- const double hood_velocity_sign =
- hood_status->velocity() * kHoodFrictionGain;
- output_struct.hood_voltage +=
- std::clamp(hood_velocity_sign, -kHoodFrictionVoltageLimit,
- kHoodFrictionVoltageLimit);
-
- // And dither the output.
- time_ += 0.00505;
- output_struct.hood_voltage += 1.3 * std::sin(time_ * 2.0 * M_PI * 30.0);
}
bool zeroed;
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index c0f3798..af15be6 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -29,9 +29,6 @@
static constexpr double kTurretFrictionGain = 10.0;
static constexpr double kTurretFrictionVoltageLimit = 1.5;
- static constexpr double kHoodFrictionGain = 40.0;
- static constexpr double kHoodFrictionVoltageLimit = 1.5;
-
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
@@ -73,8 +70,6 @@
aos::monotonic_clock::time_point shooting_start_time_ =
aos::monotonic_clock::min_time;
- double time_ = 0.0;
-
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 151146f..d6f5532 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -261,14 +261,9 @@
voltage_check_turret);
// Invert the friction model.
- const double hood_velocity_sign =
- hood_plant_->X(1) * Superstructure::kHoodFrictionGain;
::Eigen::Matrix<double, 1, 1> hood_U;
hood_U << superstructure_output_fetcher_->hood_voltage() +
- hood_plant_->voltage_offset() -
- std::clamp(hood_velocity_sign,
- -Superstructure::kHoodFrictionVoltageLimit,
- Superstructure::kHoodFrictionVoltageLimit);
+ hood_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> intake_U;
intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 4528298..0a37df6 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -141,7 +141,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), hood_pos,
- CreateProfileParameters(*builder.fbb(), 0.7, 3.0));
+ CreateProfileParameters(*builder.fbb(), 5.0, 30.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index cc1e1b8..555d16b 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -395,9 +395,9 @@
private:
void Write(const superstructure::Output &output) override {
- hood_victor_->SetSpeed(
- std::clamp(output.hood_voltage(), -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ hood_victor_->SetSpeed(std::clamp(-output.hood_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
intake_joint_victor_->SetSpeed(std::clamp(-output.intake_joint_voltage(),
-kMaxBringupPower,