Arm works
Added gravity and calibrated it. Terifying...
Change-Id: I70babb1cd3b83ddd7a81f06fb2a75cefd55bcdb8
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
new file mode 100644
index 0000000..484202e
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -0,0 +1,237 @@
+#include "y2018/control_loops/superstructure/arm/arm.h"
+
+#include <chrono>
+#include <iostream>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/arm/demo_path.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+SearchGraph::Edge MakeEdge(::std::vector<TrajectoryPair> *trajectories,
+ size_t start, size_t end, ::std::unique_ptr<Path> forwards_path,
+ ::std::unique_ptr<Path> backwards_path) {
+ trajectories->emplace_back(::std::move(forwards_path),
+ ::std::move(backwards_path), 0.005);
+
+ return SearchGraph::Edge{start, end,
+ trajectories->back().forwards.path().length()};
+}
+
+SearchGraph MakeSearchGraph(::std::vector<TrajectoryPair> *trajectories) {
+ ::std::vector<SearchGraph::Edge> edges;
+
+ // TODO(austin): Add more trajectories here.
+ edges.push_back(
+ MakeEdge(trajectories, 0, 1, MakeReversedDemoPath(), MakeDemoPath()));
+
+ return SearchGraph(2, ::std::move(edges));
+}
+
+Arm::Arm()
+ : proximal_zeroing_estimator_(constants::GetValues().arm_proximal.zeroing),
+ distal_zeroing_estimator_(constants::GetValues().arm_distal.zeroing),
+ alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
+ 0.0, 0.0, 1.0 / kAlpha1Max())
+ .finished()),
+ search_graph_(MakeSearchGraph(&trajectories_)),
+ // Go to the start of the first trajectory.
+ follower_(trajectories_[0].forwards.path().Theta(0)) {
+ // TODO(austin): Stop talking about indeces in the list and start talking
+ // about points/search for the nearest point.
+ for (TrajectoryPair &trajectory_pair : trajectories_) {
+ trajectory_pair.forwards.OptimizeTrajectory(alpha_unitizer_, kVMax());
+ trajectory_pair.backwards.OptimizeTrajectory(alpha_unitizer_, kVMax());
+ }
+}
+
+void Arm::Reset() { state_ = State::UNINITIALIZED; }
+
+void Arm::Iterate(const uint32_t *unsafe_goal,
+ const control_loops::ArmPosition *position,
+ double *proximal_output, double *distal_output,
+ bool *release_arm_brake, control_loops::ArmStatus *status) {
+ ::Eigen::Matrix<double, 2, 1> Y;
+
+ Y << position->proximal.encoder + proximal_offset_,
+ position->distal.encoder + distal_offset_;
+
+ proximal_zeroing_estimator_.UpdateEstimate(position->proximal);
+ distal_zeroing_estimator_.UpdateEstimate(position->distal);
+
+ if (proximal_output != nullptr) {
+ *proximal_output = 0.0;
+ }
+ if (distal_output != nullptr) {
+ *distal_output = 0.0;
+ }
+
+ arm_ekf_.Correct(Y, kDt());
+
+ if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= 0.05 &&
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= 0.05) {
+ close_enough_for_full_power_ = true;
+ }
+ if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) >= 1.10 ||
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) >= 1.10) {
+ close_enough_for_full_power_ = false;
+ }
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Wait in the uninitialized state until the intake is initialized.
+ LOG(DEBUG, "Uninitialized, waiting for intake\n");
+ state_ = State::ZEROING;
+ proximal_zeroing_estimator_.Reset();
+ distal_zeroing_estimator_.Reset();
+ // TODO(austin): Go to the nearest node. For now, we are always going to
+ // go to node 0.
+ break;
+
+ case State::ZEROING:
+ // Zero by not moving.
+ if (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed()) {
+ state_ = State::RUNNING;
+
+ proximal_offset_ = proximal_zeroing_estimator_.offset();
+ distal_offset_ = distal_zeroing_estimator_.offset();
+
+ Y << position->proximal.encoder + proximal_offset_,
+ position->distal.encoder + distal_offset_;
+
+ // TODO(austin): Offset ekf rather than reset it. Since we aren't
+ // moving at this point, it's pretty safe to do this.
+ ::Eigen::Matrix<double, 4, 1> X;
+ X << Y(0), 0.0, Y(1), 0.0;
+ arm_ekf_.Reset(X);
+ } else {
+ break;
+ }
+
+ case State::RUNNING:
+ // ESTOP if we hit the hard limits.
+ // TODO(austin): Pick some sane limits.
+ if (proximal_zeroing_estimator_.error() ||
+ distal_zeroing_estimator_.error()) {
+ LOG(ERROR, "Zeroing error ESTOP\n");
+ state_ = State::ESTOP;
+ } else if (unsafe_goal != nullptr) {
+ if (!follower_.has_path()) {
+ // If we don't have a path and are far from the goal, don't update the
+ // path.
+ // TODO(austin): Once we get close to our first setpoint, crank the
+ // power back up. Otherwise we'll be weak at startup.
+ LOG(INFO, "No path.\n");
+ if (!close_enough_for_full_power_) {
+ break;
+ }
+ }
+ if (current_node_ != *unsafe_goal) {
+ LOG(INFO, "Goal is different\n");
+ if (*unsafe_goal >= search_graph_.num_vertexes()) {
+ LOG(ERROR, "goal out of range ESTOP\n");
+ state_ = State::ESTOP;
+ break;
+ }
+
+ if (follower_.path_distance_to_go() > 1e-3) {
+ LOG(INFO, " Distance to go %f\n", follower_.path_distance_to_go());
+ // Still on the old path segment. Can't change yet.
+ break;
+ }
+
+ search_graph_.SetGoal(*unsafe_goal);
+
+ size_t min_edge = 0;
+ double min_cost = -1.0;
+ for (const SearchGraph::HalfEdge &edge :
+ search_graph_.Neighbors(current_node_)) {
+ const double cost = search_graph_.GetCostToGoal(edge.dest);
+ if (min_cost == -1 || cost < min_cost) {
+ min_edge = edge.edge_id;
+ min_cost = cost;
+ }
+ }
+ // Ok, now we know which edge we are on. Figure out the path and
+ // trajectory.
+ const SearchGraph::Edge &next_edge = search_graph_.edges()[min_edge];
+ if (next_edge.start == current_node_) {
+ follower_.SwitchTrajectory(&trajectories_[min_edge].forwards);
+ current_node_ = next_edge.end;
+ } else {
+ follower_.SwitchTrajectory(&trajectories_[min_edge].backwards);
+ current_node_ = next_edge.start;
+ }
+ }
+ }
+ break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ break;
+ }
+
+ const bool disable =
+ ((proximal_output == nullptr) || (distal_output == nullptr) ||
+ (release_arm_brake == nullptr)) ||
+ state_ != State::RUNNING;
+ if (disable) {
+ close_enough_for_full_power_ = false;
+ }
+
+ follower_.Update(
+ arm_ekf_.X_hat(), disable, kDt(), kVMax(),
+ close_enough_for_full_power_ ? kOperatingVoltage() : kPathlessVMax());
+ LOG(INFO, "Max voltage: %f\n",
+ close_enough_for_full_power_ ? kOperatingVoltage() : kPathlessVMax());
+ status->goal_theta0 = follower_.theta(0);
+ status->goal_theta1 = follower_.theta(1);
+ status->goal_omega0 = follower_.omega(0);
+ status->goal_omega1 = follower_.omega(1);
+
+ status->theta0 = arm_ekf_.X_hat(0);
+ status->theta1 = arm_ekf_.X_hat(2);
+ status->omega0 = arm_ekf_.X_hat(1);
+ status->omega1 = arm_ekf_.X_hat(3);
+ status->voltage_error0 = arm_ekf_.X_hat(4);
+ status->voltage_error1 = arm_ekf_.X_hat(5);
+
+ if (!disable) {
+ *proximal_output = ::std::max(
+ -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(0)));
+ *distal_output = ::std::max(
+ -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
+ *release_arm_brake = true;
+ }
+
+ status->proximal_estimator_state =
+ proximal_zeroing_estimator_.GetEstimatorState();
+ status->distal_estimator_state =
+ distal_zeroing_estimator_.GetEstimatorState();
+
+ status->path_distance_to_go = follower_.path_distance_to_go();
+ status->current_node = current_node_;
+
+ status->zeroed = (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed());
+ status->estopped = (state_ == State::ESTOP);
+ status->state = static_cast<int32_t>(state_);
+ status->failed_solutions = follower_.failed_solutions();
+
+ arm_ekf_.Predict(follower_.U(), kDt());
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018