Per path max voltage.
Change-Id: Iee719699ec392192330c92c9eaf9596fa0d9c250
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 4912359..d3997ca 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -31,7 +31,7 @@
int i = 0;
for (const auto &trajectory : trajectories_) {
LOG(INFO, "trajectory length for edge node %d: %f\n", i,
- trajectory.path().length());
+ trajectory.trajectory.path().length());
++i;
}
}
@@ -330,7 +330,8 @@
LOG(INFO, "Switching from node %d to %d along edge %d\n",
static_cast<int>(current_node_), static_cast<int>(next_edge.end),
static_cast<int>(min_edge));
- follower_.SwitchTrajectory(&trajectories_[min_edge]);
+ vmax_ = trajectories_[min_edge].vmax;
+ follower_.SwitchTrajectory(&trajectories_[min_edge].trajectory);
current_node_ = next_edge.end;
}
}
@@ -340,7 +341,7 @@
close_enough_for_full_power_
? kOperatingVoltage()
: (state_ == State::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
- follower_.Update(arm_ekf_.X_hat(), disable, kDt(), kVMax(),
+ follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
max_operating_voltage);
LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
status->goal_theta0 = follower_.theta(0);
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 840ce53..d86b95b 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -5,6 +5,7 @@
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/arm/graph.h"
#include "y2018/control_loops/superstructure/arm/trajectory.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
@@ -23,15 +24,15 @@
// The operating voltage.
static constexpr double kOperatingVoltage() {
- return kGrannyMode() ? 4.0 : 12.0;
+ return kGrannyMode() ? 5.0 : 12.0;
}
static constexpr double kDt() { return 0.00505; }
- static constexpr double kAlpha0Max() { return kGrannyMode() ? 10.0 : 10.0; }
- static constexpr double kAlpha1Max() { return kGrannyMode() ? 10.0 : 10.0; }
+ static constexpr double kAlpha0Max() { return kGrannyMode() ? 5.0 : 15.0; }
+ static constexpr double kAlpha1Max() { return kGrannyMode() ? 5.0 : 15.0; }
static constexpr double kVMax() { return kGrannyMode() ? 5.0 : 11.5; }
static constexpr double kPathlessVMax() { return 5.0; }
- static constexpr double kGotoPathVMax() { return 12.0; }
+ static constexpr double kGotoPathVMax() { return 6.0; }
void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
const control_loops::ArmPosition *position,
@@ -96,7 +97,9 @@
const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
- ::std::vector<Trajectory> trajectories_;
+ double vmax_ = kVMax();
+
+ ::std::vector<TrajectoryAndParams> trajectories_;
SearchGraph search_graph_;
bool close_enough_for_full_power_ = false;