Arm works
Added gravity and calibrated it. Terifying...
Change-Id: I70babb1cd3b83ddd7a81f06fb2a75cefd55bcdb8
diff --git a/y2018/control_loops/superstructure/arm/graph.cc b/y2018/control_loops/superstructure/arm/graph.cc
index a1e6fb2..6544a8e 100644
--- a/y2018/control_loops/superstructure/arm/graph.cc
+++ b/y2018/control_loops/superstructure/arm/graph.cc
@@ -8,11 +8,14 @@
namespace superstructure {
namespace arm {
-SearchGraph::SearchGraph(size_t num_vertexes, const std::vector<Edge> &edges)
- : edges_(edges) {
+SearchGraph::SearchGraph(size_t num_vertexes, std::initializer_list<Edge> edges)
+ : SearchGraph(num_vertexes, ::std::move(::std::vector<Edge>(edges))) {}
+
+SearchGraph::SearchGraph(size_t num_vertexes, std::vector<Edge> &&edges)
+ : edges_(::std::move(edges)) {
vertexes_.resize(num_vertexes);
size_t i = 0;
- for (const auto &edge : edges) {
+ for (const auto &edge : edges_) {
assert(edge.start < num_vertexes);
assert(edge.end < num_vertexes);
assert(edge.start != edge.end);
@@ -22,15 +25,19 @@
++i;
}
for (auto &vertex : vertexes_) {
- std::sort(vertex.forward.begin(), vertex.forward.end());
- std::sort(vertex.reverse.begin(), vertex.reverse.end());
+ ::std::sort(vertex.forward.begin(), vertex.forward.end());
+ ::std::sort(vertex.reverse.begin(), vertex.reverse.end());
}
vertex_heap_.Reserve(num_vertexes);
+ SetGoal(0);
}
SearchGraph::~SearchGraph() {}
void SearchGraph::SetGoal(size_t vertex) {
+ if (last_searched_vertex_ == vertex) {
+ return;
+ }
assert(vertex < vertexes_.size());
vertex_heap_.Clear();
@@ -52,6 +59,7 @@
}
}
}
+ last_searched_vertex_ = vertex;
}
double SearchGraph::GetCostToGoal(size_t vertex) {