Add battery voltage compensation to the catapult
This appears to help a lot with shot accuracy.
Change-Id: I91c100196f8fd8dd98d98ed48a9ac0a60578bcb6
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index fda62ad..c43a777 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -313,7 +313,7 @@
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
Catapult::Iterate(const Goal *unsafe_goal, const Position *position,
- double *catapult_voltage, bool fire,
+ double battery_voltage, double *catapult_voltage, bool fire,
flatbuffers::FlatBufferBuilder *fbb) {
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*catapult_goal = unsafe_goal != nullptr && unsafe_goal->has_catapult()
@@ -373,8 +373,9 @@
} else {
// TODO(austin): Voltage error?
CHECK_NOTNULL(catapult_voltage);
- *catapult_voltage =
- std::max(0.0, std::min(12.0, *solution - 0.0 * next_X(2, 0)));
+ *catapult_voltage = std::max(
+ 0.0, std::min(12.0, (*solution - 0.0 * next_X(2, 0)) * 12.0 /
+ std::max(battery_voltage, 8.0)));
use_profile_ = false;
}
} else {
@@ -420,8 +421,9 @@
}
}
- catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage
- : 0.0);
+ catapult_.UpdateObserver(catapult_voltage != nullptr
+ ? (*catapult_voltage * battery_voltage / 12.0)
+ : 0.0);
return catapult_.MakeStatus(fbb);
}
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index ccd4b06..0606c0d 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -211,7 +211,7 @@
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
Iterate(const Goal *unsafe_goal, const Position *position,
- double *catapult_voltage, bool fire,
+ double battery_voltage, double *catapult_voltage, bool fire,
flatbuffers::FlatBufferBuilder *fbb);
private:
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index c87cc95..0b693ef 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -394,12 +394,12 @@
// Disable the catapult if we want to restart to prevent damage with
// flippers
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
- catapult_status_offset =
- catapult_.Iterate(unsafe_goal, position,
- output != nullptr && !catapult_.estopped()
- ? &(output_struct.catapult_voltage)
- : nullptr,
- fire_, status->fbb());
+ catapult_status_offset = catapult_.Iterate(
+ unsafe_goal, position, robot_state().voltage_battery(),
+ output != nullptr && !catapult_.estopped()
+ ? &(output_struct.catapult_voltage)
+ : nullptr,
+ fire_, status->fbb());
const flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
climber_status_offset = climber_.Iterate(