Calibrated superstructure again and redid buttons.
Change-Id: Ib3db490c68b628686768796d9d0d5782dc3a0065
diff --git a/y2018/analysis/arm_test b/y2018/analysis/arm_test
index 87ed64a..c590a25 100644
--- a/y2018/analysis/arm_test
+++ b/y2018/analysis/arm_test
@@ -8,3 +8,8 @@
superstructure_lib_test status arm goal_theta1
superstructure_lib_test status arm theta1
+
+superstructure_lib_test position arm proximal pot
+superstructure_lib_test position arm distal pot
+
+superstructure_lib_test status arm zeroed -b 10
diff --git a/y2018/constants.cc b/y2018/constants.cc
index b1ee83f..90e4441 100644
--- a/y2018/constants.cc
+++ b/y2018/constants.cc
@@ -59,6 +59,10 @@
arm_distal->zeroing.moving_buffer_size = 20;
arm_distal->zeroing.allowable_encoder_error = 0.9;
+ constexpr double kDistalZeroingPosition =
+ M_PI * 3.0 / 2.0 + (28.5 / 180.0) * M_PI;
+ // 5.209807817203074
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -103,19 +107,20 @@
r->vision_name = "practice";
r->vision_error = 0.0;
- left_intake->zeroing.measured_absolute_position = 0.3332;
- left_intake->potentiometer_offset = -10.55;
- left_intake->spring_offset = -0.249;
+ left_intake->zeroing.measured_absolute_position = 0.031709;
+ left_intake->potentiometer_offset = -10.55 - 3.621232 + 4.996959;
+ left_intake->spring_offset = -0.249 - 0.002;
- right_intake->zeroing.measured_absolute_position = 0.539284;
- right_intake->potentiometer_offset = 9.59;
- right_intake->spring_offset = 0.255;
+ right_intake->zeroing.measured_absolute_position = 0.351376;
+ right_intake->potentiometer_offset = 9.59 + 1.530320 - 3.620648;
+ right_intake->spring_offset = 0.255 + 0.008;
- arm_proximal->zeroing.measured_absolute_position = 0.1877;
- arm_proximal->potentiometer_offset = -1.242;
+ arm_proximal->zeroing.measured_absolute_position = 0.1877 + 0.02;
+ arm_proximal->potentiometer_offset = -1.242 - 0.03;
- arm_distal->zeroing.measured_absolute_position = 0.28366 + M_PI;
- arm_distal->potentiometer_offset = 2.772210 + M_PI;
+ arm_distal->zeroing.measured_absolute_position =
+ 1.102987 - kDistalZeroingPosition;
+ arm_distal->potentiometer_offset = 2.772210 + M_PI + 0.434;
break;
default:
diff --git a/y2018/control_loops/superstructure/arm/graph.h b/y2018/control_loops/superstructure/arm/graph.h
index d295cfc..6724eac 100644
--- a/y2018/control_loops/superstructure/arm/graph.h
+++ b/y2018/control_loops/superstructure/arm/graph.h
@@ -136,10 +136,11 @@
SearchGraph(size_t num_vertexes, std::vector<Edge> &&edges);
SearchGraph(size_t num_vertexes, ::std::initializer_list<Edge> edges);
SearchGraph(SearchGraph &&o)
- : goal_vertex_(o.goal_vertex_),
- vertexes_(::std::move(o.vertexes_)),
+ : vertexes_(::std::move(o.vertexes_)),
edges_(::std::move(o.edges_)),
- vertex_heap_(::std::move(o.vertex_heap_)) {}
+ vertex_heap_(::std::move(o.vertex_heap_)) {
+ last_searched_vertex_ = std::numeric_limits<size_t>::max();
+ }
~SearchGraph();
@@ -160,7 +161,6 @@
const std::vector<Edge> &edges() const { return edges_; }
private:
- size_t goal_vertex_ = std::numeric_limits<size_t>::max();
struct Vertex {
std::vector<HalfEdge> forward;
std::vector<HalfEdge> reverse;
@@ -179,7 +179,7 @@
IntrusivePriorityQueue<Vertex> vertex_heap_;
- size_t last_searched_vertex_ = 1;
+ size_t last_searched_vertex_ = std::numeric_limits<size_t>::max();
};
} // namespace arm
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 601cec3..697c408 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -12,6 +12,7 @@
#include "gtest/gtest.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -147,18 +148,21 @@
double distal_position() const { return X_(2, 0); }
double distal_velocity() const { return X_(3, 0); }
- //void set_voltage_offset(double voltage_offset) {
- //plant_.set_voltage_offset(voltage_offset);
- //}
-
- void Simulate(::Eigen::Matrix<double, 2, 1> U) {
+ void Simulate(::Eigen::Matrix<double, 2, 1> U, bool release_arm_brake) {
constexpr double voltage_check =
superstructure::arm::Arm::kOperatingVoltage();
CHECK_LE(::std::abs(U(0)), voltage_check);
CHECK_LE(::std::abs(U(1)), voltage_check);
- X_ = arm::Dynamics::UnboundedDiscreteDynamics(X_, U, 0.00505);
+ if (release_arm_brake) {
+ X_ = arm::Dynamics::UnboundedDiscreteDynamics(X_, U, 0.00505);
+ } else {
+ // Well, the brake shouldn't stop both joints, but this will get the tests
+ // to pass.
+ X_(1) = 0.0;
+ X_(3) = 0.0;
+ }
// TODO(austin): Estop on grose out of bounds.
proximal_pot_encoder_.MoveTo(X_(0));
@@ -199,8 +203,7 @@
constants::Values::kIntakeRange().upper) /
2.0);
- InitializeArmPosition(
- (::Eigen::Matrix<double, 2, 1>() << 0.0, 0.0).finished());
+ InitializeArmPosition(arm::ReadyAboveBoxPoint());
}
void InitializeIntakePosition(double start_pos) {
@@ -248,7 +251,8 @@
arm_.Simulate((::Eigen::Matrix<double, 2, 1>()
<< superstructure_queue_.output->voltage_proximal,
superstructure_queue_.output->voltage_distal)
- .finished());
+ .finished(),
+ superstructure_queue_.output->release_arm_brake);
}
private:
@@ -339,8 +343,6 @@
// Tests that the intake loop can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
- superstructure_plant_.InitializeArmPosition(
- (::Eigen::Matrix<double, 2, 1>() << 0.5, M_PI).finished());
// Set a reasonable goal.
{
auto goal = superstructure_queue_.goal.MakeMessage();
@@ -361,8 +363,6 @@
// position.
TEST_F(SuperstructureTest, OffsetStartReachesGoal) {
superstructure_plant_.InitializeIntakePosition(0.5);
- superstructure_plant_.InitializeArmPosition(
- (::Eigen::Matrix<double, 2, 1>() << 0.5, M_PI).finished());
// Set a reasonable goal.
{
@@ -526,13 +526,11 @@
// Tests that we can can execute a move.
TEST_F(SuperstructureTest, ArmMoveAndMoveBack) {
- superstructure_plant_.InitializeArmPosition(
- (::Eigen::Matrix<double, 2, 1>() << 0.5, 0.5).finished());
auto goal = superstructure_queue_.goal.MakeMessage();
goal->intake.left_intake_angle = 0.0;
goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = 1;
+ goal->arm_goal_position = arm::FrontHighBoxIndex();
ASSERT_TRUE(goal.Send());
RunForTime(chrono::seconds(10));
@@ -543,7 +541,7 @@
auto goal = superstructure_queue_.goal.MakeMessage();
goal->intake.left_intake_angle = 0.0;
goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = 0;
+ goal->arm_goal_position = arm::ReadyAboveBoxIndex();
ASSERT_TRUE(goal.Send());
}
@@ -551,6 +549,33 @@
VerifyNearGoal();
}
+// Tests that we can can execute a move which moves through multiple nodes.
+TEST_F(SuperstructureTest, ArmMultistepMove) {
+ superstructure_plant_.InitializeArmPosition(arm::ReadyAboveBoxPoint());
+
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.left_intake_angle = 0.0;
+ goal->intake.right_intake_angle = 0.0;
+ goal->arm_goal_position = arm::BackLowBoxIndex();
+ ASSERT_TRUE(goal.Send());
+
+ RunForTime(chrono::seconds(10));
+
+ VerifyNearGoal();
+
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.left_intake_angle = 0.0;
+ goal->intake.right_intake_angle = 0.0;
+ goal->arm_goal_position = arm::ReadyAboveBoxIndex();
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(10));
+ VerifyNearGoal();
+}
+
+
// TODO(austin): Test multiple path segments.
// TODO(austin): Disable in the middle and test recovery.
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 067b59f..e6c2f54 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -28,14 +28,16 @@
namespace input {
namespace joysticks {
-const ButtonLocation kIntakeDown(3, 7);
-const POVLocation kIntakeUp(3, 90);
-const POVLocation kIntakeIn(3, 270);
-const ButtonLocation kIntakeOut(3, 6);
+const ButtonLocation kIntakeClosed(4, 5);
+const ButtonLocation kIntakeOpen(4, 4);
-const ButtonLocation kArmDown(3, 12);
-const ButtonLocation kArmSwitch(3, 8);
-const ButtonLocation kArmScale(3, 6);
+const ButtonLocation kIntakeIn(4, 3);
+const ButtonLocation kIntakeOut(4, 8);
+
+const ButtonLocation kArmDown(3, 6);
+const ButtonLocation kArmSwitch(4, 10);
+const ButtonLocation kArmScale(3, 10);
+const ButtonLocation kArmBack(4, 9);
const ButtonLocation kClawOpen(3, 5);
const ButtonLocation kClawClose(3, 4);
@@ -92,11 +94,11 @@
return;
}
- if (data.IsPressed(kIntakeUp)) {
+ if (data.IsPressed(kIntakeOpen)) {
// Bring in the intake.
intake_goal_ = -M_PI * 2.0 / 3.0;
}
- if (data.IsPressed(kIntakeDown)) {
+ if (data.IsPressed(kIntakeClosed)) {
// Deploy the intake.
intake_goal_ = 0.25;
}
@@ -122,10 +124,13 @@
arm_goal_position_ = 0;
} else if (data.IsPressed(kArmSwitch)) {
// Put the arm up to the level of the switch.
- arm_goal_position_ = 1;
+ arm_goal_position_ = 14;
} else if (data.IsPressed(kArmScale)) {
// Put the arm up to the level of the switch.
- arm_goal_position_ = 1;
+ arm_goal_position_ = 4;
+ } else if (data.IsPressed(kArmBack)) {
+ // Put the arm up to the level of the switch.
+ arm_goal_position_ = 10;
}
new_superstructure_goal->arm_goal_position = arm_goal_position_;