Merge "Fix superstructure zeroing corner case"
diff --git a/aos/common/time.cc b/aos/common/time.cc
index 87c991d..125b016 100644
--- a/aos/common/time.cc
+++ b/aos/common/time.cc
@@ -12,7 +12,45 @@
#include "aos/common/logging/logging.h"
#include "aos/common/mutex.h"
+namespace std {
+namespace this_thread {
+template <>
+void sleep_until(const ::aos::monotonic_clock::time_point &end_time) {
+ struct timespec end_time_timespec;
+ ::std::chrono::seconds sec =
+ ::std::chrono::duration_cast<::std::chrono::seconds>(
+ end_time.time_since_epoch());
+ ::std::chrono::nanoseconds nsec =
+ ::std::chrono::duration_cast<::std::chrono::nanoseconds>(
+ end_time.time_since_epoch() - sec);
+ end_time_timespec.tv_sec = sec.count();
+ end_time_timespec.tv_nsec = nsec.count();
+ int returnval;
+ do {
+ returnval = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME,
+ &end_time_timespec, nullptr);
+ if (returnval != EINTR && returnval != 0) {
+ PLOG(FATAL, "clock_nanosleep(%jd, TIMER_ABSTIME, %p, nullptr) failed",
+ static_cast<uintmax_t>(CLOCK_MONOTONIC), &end_time_timespec);
+ }
+ } while (returnval != 0);
+}
+
+} // namespace this_thread
+} // namespace std
+
+
namespace aos {
+monotonic_clock::time_point monotonic_clock::now() noexcept {
+ struct timespec current_time;
+ if (clock_gettime(CLOCK_MONOTONIC, ¤t_time) != 0) {
+ PLOG(FATAL, "clock_gettime(%jd, %p) failed",
+ static_cast<uintmax_t>(CLOCK_MONOTONIC), ¤t_time);
+ }
+ return time_point(::std::chrono::seconds(current_time.tv_sec) +
+ ::std::chrono::nanoseconds(current_time.tv_nsec));
+}
+
namespace time {
// State required to enable and use mock time.
diff --git a/aos/common/time.h b/aos/common/time.h
index a176ba0..40b72b0 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -7,12 +7,31 @@
#include <stdint.h>
#include <type_traits>
+#include <chrono>
+#include <thread>
#include <ostream>
#include "aos/common/type_traits.h"
#include "aos/common/macros.h"
namespace aos {
+
+class monotonic_clock {
+ public:
+ typedef ::std::chrono::nanoseconds::rep rep;
+ typedef ::std::chrono::nanoseconds::period period;
+ typedef ::std::chrono::nanoseconds duration;
+ typedef ::std::chrono::time_point<monotonic_clock> time_point;
+
+ static monotonic_clock::time_point now() noexcept;
+ static constexpr bool is_steady = true;
+
+ // Returns the epoch (0).
+ static constexpr monotonic_clock::time_point epoch() {
+ return time_point(duration(0));
+ }
+};
+
namespace time {
// A nice structure for representing times.
@@ -280,4 +299,15 @@
} // namespace time
} // namespace aos
+namespace std {
+namespace this_thread {
+// Template specialization for monotonic_clock, since we can use clock_nanosleep
+// with TIMER_ABSTIME and get very precise absolute time sleeps.
+template <>
+void sleep_until(const ::aos::monotonic_clock::time_point &end_time);
+
+} // namespace this_thread
+} // namespace std
+
+
#endif // AOS_COMMON_TIME_H_
diff --git a/aos/common/time_test.cc b/aos/common/time_test.cc
index 008b3cf..3ae82ab 100644
--- a/aos/common/time_test.cc
+++ b/aos/common/time_test.cc
@@ -1,5 +1,7 @@
#include "aos/common/time.h"
+#include <thread>
+
#include "gtest/gtest.h"
#include "aos/common/macros.h"
@@ -256,6 +258,16 @@
EXPECT_EQ(MACRO_DARG(Time(0, Time::kNSecInSec / 100)), Time::FromRate(100));
}
+// Test the monotonic_clock and sleep_until functions.
+TEST(TimeTest, MonotonicClockSleepAndNow) {
+ monotonic_clock::time_point start = monotonic_clock::now();
+ const auto kSleepTime = ::std::chrono::milliseconds(500);
+ ::std::this_thread::sleep_until(start + kSleepTime);
+ monotonic_clock::time_point end = monotonic_clock::now();
+ EXPECT_GE(end - start, kSleepTime);
+ EXPECT_LT(end - start, kSleepTime + ::std::chrono::milliseconds(200));
+}
+
} // namespace testing
} // namespace time
} // namespace aos
diff --git a/aos/testing/BUILD b/aos/testing/BUILD
index 8656d23..a8dce04 100644
--- a/aos/testing/BUILD
+++ b/aos/testing/BUILD
@@ -72,3 +72,15 @@
],
testonly = True,
)
+
+cc_library(
+ name = 'random_seed',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'random_seed.cc',
+ ],
+ hdrs = [
+ 'random_seed.h',
+ ],
+ testonly = True,
+)
diff --git a/aos/testing/random_seed.cc b/aos/testing/random_seed.cc
new file mode 100644
index 0000000..9e7136f
--- /dev/null
+++ b/aos/testing/random_seed.cc
@@ -0,0 +1,17 @@
+#include "aos/testing/random_seed.h"
+
+#include <stdlib.h>
+
+namespace aos {
+namespace testing {
+
+int RandomSeed() {
+ const char *from_environment = getenv("TEST_RANDOM_SEED");
+ if (from_environment != nullptr) {
+ return atoi(from_environment);
+ }
+ return 1;
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/testing/random_seed.h b/aos/testing/random_seed.h
new file mode 100644
index 0000000..825e7b1
--- /dev/null
+++ b/aos/testing/random_seed.h
@@ -0,0 +1,15 @@
+#ifndef AOS_TESTING_RANDOM_SEED_H_
+#define AOS_TESTING_RANDOM_SEED_H_
+
+namespace aos {
+namespace testing {
+
+// Returns the random seed to use for testing.
+//
+// This is ${TEST_RANDOM_SEED} if it is set or 1.
+int RandomSeed();
+
+} // namespace testing
+} // namespace aos
+
+#endif // AOS_TESTING_RANDOM_SEED_H_
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 5eeed1d..9dee424 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -59,6 +59,7 @@
cc_library(
name = 'position_sensor_sim',
+ testonly = True,
srcs = [
'position_sensor_sim.cc',
],
@@ -69,6 +70,7 @@
':queues',
':gaussian_noise',
'//debian:libm',
+ '//aos/testing:random_seed',
],
)
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index caa383e..6075155 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -70,6 +70,11 @@
index_count_++;
}
+ if (new_index_segment != cur_index_segment_) {
+ latched_pot_ = pot_noise_.AddNoiseToSample(cur_index_ * index_diff_ +
+ known_index_pos_);
+ }
+
cur_index_segment_ = new_index_segment;
cur_pos_ = new_pos;
}
@@ -86,7 +91,7 @@
double index_pulse_position = cur_index_ * index_diff_ + known_index_pos_;
// Populate the latched pot/encoder samples.
- values->latched_pot = pot_noise_.AddNoiseToSample(index_pulse_position);
+ values->latched_pot = latched_pot_;
values->latched_encoder = index_pulse_position - start_position_;
}
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 6c1884d..3ce3056 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -1,6 +1,8 @@
#ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
#define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+#include "aos/testing/random_seed.h"
+
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/gaussian_noise.h"
@@ -18,7 +20,8 @@
// noise_seed: The seed to feed into the random number generator for the
// potentiometer values.
// TODO(danielp): Allow for starting with a non-zero encoder value.
- PositionSensorSimulator(double index_diff, unsigned int noise_seed = 0);
+ PositionSensorSimulator(double index_diff, unsigned int noise_seed =
+ ::aos::testing::RandomSeed());
// Set new parameters for the sensors. This is useful for unit tests to change
// the simulated sensors' behavior on the fly.
@@ -57,6 +60,8 @@
int cur_index_;
// How many index pulses we've seen.
int index_count_;
+ // The pot position at the most recent index pulse with noise added.
+ double latched_pot_;
// Distance between index pulses on the mechanism.
double index_diff_;
// Absolute position of a known index pulse.
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 0c9d3a9..1ca5033 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -150,5 +150,50 @@
EXPECT_DOUBLE_EQ(index_diff * 1.25, position.latched_encoder);
}
+// Tests that the latched values update correctly.
+TEST_F(PositionSensorSimTest, LatchedValues) {
+ const double index_diff = 0.5;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(0, 0.25);
+ PotAndIndexPosition position;
+
+ sim.MoveTo(0.75 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(0u, position.index_pulses);
+
+ sim.MoveTo(1.75 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(1u, position.index_pulses);
+ EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
+ EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+ const double first_latched_pot = position.latched_pot;
+
+ sim.MoveTo(1.95 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(1u, position.index_pulses);
+ EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
+ EXPECT_DOUBLE_EQ(first_latched_pot, position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+
+ sim.MoveTo(2.05 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(2u, position.index_pulses);
+ EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
+ EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+
+ sim.MoveTo(1.95 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(3u, position.index_pulses);
+ EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
+ EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+
+ sim.MoveTo(0.95 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(4u, position.index_pulses);
+ EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
+ EXPECT_GT(::std::abs(first_latched_pot - position.latched_pot), 0.005);
+ EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+}
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 2284bde..4ff6391 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -108,8 +108,7 @@
// zeroed.
zeroed_ = true;
// Throw an error if first_start_pos is bigger/smaller than
- // allowable_encoder_error_ * index_diff +
- // start_pos.
+ // allowable_encoder_error_ * index_diff + start_pos.
if (::std::abs(first_start_pos_ - start_pos_) >
allowable_encoder_error_ * index_diff_) {
if (!error_) {
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 5216df0..b5e2c14 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -116,8 +116,8 @@
// Stores the position "start_pos" variable the first time the program
// is zeroed.
double first_start_pos_;
- // Value between 0 and 1 which determines a fraction of the index_diff
- // you want to use.
+ // The fraction of index_diff (possibly greater than 1) after which an error
+ // is reported.
double allowable_encoder_error_;
};
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 1db8404..199dda5 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -39,8 +39,6 @@
const ProfileParameters kTwoBallReturnSlow = {3.0, 2.5};
const ProfileParameters kTwoBallBallPickup = {2.0, 1.75};
const ProfileParameters kTwoBallBallPickupAccel = {2.0, 2.5};
-
-const double kDistanceShort = 0.25;
} // namespace
AutonomousActor::AutonomousActor(actors::AutonomousActionQueueGroup *s)
diff --git a/y2016/constants.cc b/y2016/constants.cc
index debbc7c..d692c3d 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -86,25 +86,25 @@
-4.550531 + 150.40906362 * M_PI / 180.0 + 0.5098 - 0.0178 - 0.0725,
{Values::kZeroingSampleSize, Values::kIntakeEncoderIndexDifference,
// Location of an index pulse.
- 0.018008, 1.5},
+ 0.018008, 2.5},
},
// Shoulder
{
// Value to add to the pot reading for the shoulder.
-1.0 - 0.0822 + 0.06138835 * M_PI / 180.0 - 0.0323 - 0.1057 +
- 0.0035 + 0.0055 - 0.001,
+ 0.0035 + 0.0055 - 0.001 - 0.0103 + 0.0032,
{Values::kZeroingSampleSize, Values::kShoulderEncoderIndexDifference,
- 0.536989, 1.5},
+ 0.535359, 2.5},
},
// Wrist
{
// Value to add to the pot reading for the wrist.
3.2390714288298668 + -0.06138835 * M_PI / 180.0 + 0.0078 - 0.0548 -
- 0.0167 + 0.002 - 0.0026 - 0.1040 - 0.0035 - 0.0012,
+ 0.0167 + 0.002 - 0.0026 - 0.1040 - 0.0035 - 0.0012 + 0.0166 - 0.017 + 0.148 + 0.004,
{Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
- -0.054882, 1.5},
+ -0.354946, 2.5},
},
0.0,
@@ -126,17 +126,18 @@
// Shoulder (Now calibrated at 0)
{
- -1.0016 - 0.0841 + 0.06138835 * M_PI / 180.0 + 1.07838 - 1.0441 + 0.0034,
+ -1.0016 - 0.0841 + 0.06138835 * M_PI / 180.0 + 1.07838 - 1.0441 +
+ 0.0034 + 0.0065,
{Values::kZeroingSampleSize, Values::kShoulderEncoderIndexDifference,
- 0.416092, 1.3},
+ 0.126458, 1.3},
},
// Wrist
{
3.326328571170133 - 0.06138835 * M_PI / 180.0 - 0.177 + 0.0323 -
- 0.023 + 0.0488,
+ 0.023 + 0.0488 + 0.0120 - 0.0005 - 0.0784 - 0.0010 - 0.080 + 0.1245,
{Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
- -0.005145, 1.3},
+ -0.263227, 1.3},
},
0.011,
diff --git a/y2016/constants.h b/y2016/constants.h
index 972d5a3..d52555c 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -54,7 +54,7 @@
// Subsystem motion ranges, in whatever units that their respective queues say
// the use.
static constexpr ::frc971::constants::Range kIntakeRange{// Lower hard stop
- -0.4,
+ -0.5,
// Upper hard stop
2.85 + 0.05,
// Lower soft stop
@@ -62,7 +62,7 @@
// Uppper soft stop
2.725};
static constexpr ::frc971::constants::Range kShoulderRange{// Lower hard stop
- -0.100,
+ -0.150,
// Upper hard stop
2.8,
// Lower soft stop
@@ -70,9 +70,9 @@
// Uppper soft stop
2.0};
static constexpr ::frc971::constants::Range kWristRange{// Lower hard stop
- -2.9,
+ -3.0,
// Upper hard stop
- 2.9,
+ 3.0,
// Lower soft stop
-2.6,
// Uppper soft stop
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 4aa773f..7e4bdac 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -634,7 +634,7 @@
? kOperatingVoltage
: kZeroingVoltage;
if (unsafe_goal) {
- constexpr float kTriggerThreshold = 12.0 * 0.25 / 0.005;
+ constexpr float kTriggerThreshold = 12.0 * 0.90 / 0.005;
if (unsafe_goal->voltage_climber > 1.0) {
kill_shoulder_accumulator_ +=
@@ -647,8 +647,11 @@
kill_shoulder_ = true;
}
}
- arm_.set_max_voltage(kill_shoulder_ ? 0.0 : max_voltage,
- kill_shoulder_ ? kShooterHangingVoltage : max_voltage);
+ arm_.set_max_voltage(
+ kill_shoulder_ ? 0.0 : max_voltage,
+ kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
+ : kShooterHangingVoltage)
+ : max_voltage);
intake_.set_max_voltage(max_voltage);
if (IsRunning() && !kill_shoulder_) {
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 0fb851a..eb16405 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -112,6 +112,7 @@
static constexpr double kZeroingVoltage = 6.0;
static constexpr double kShooterHangingVoltage = 6.0;
+ static constexpr double kShooterHangingLowVoltage = 2.0;
static constexpr double kOperatingVoltage = 12.0;
static constexpr double kLandingShoulderDownVoltage = -1.5;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 9fdec26..02f7ae2 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -795,7 +795,7 @@
RunForTime(Time::InSeconds(2), false);
EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
- superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
+ superstructure_plant_.set_power_error(1.0, 1.5, 1.0);
RunForTime(Time::InSeconds(1), false);
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index e1e0c9b..45733ff 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -222,7 +222,7 @@
if (data.IsPressed(kFrontLong)) {
// Forwards shot
shoulder_goal_ = M_PI / 2.0 + 0.1;
- wrist_goal_ = M_PI + 0.41 + 0.02;
+ wrist_goal_ = M_PI + 0.41 + 0.02 - 0.005;
if (drivetrain_queue.status.get()) {
wrist_goal_ += drivetrain_queue.status->ground_angle;
}
@@ -239,15 +239,18 @@
intake_goal_ = intake_when_shooting;
} else if (data.IsPressed(kBackFender)) {
// Fender shot back
- shoulder_goal_ = 0.65;
- wrist_goal_ = -1.20;
- shooter_velocity_ = 550.0;
+ shoulder_goal_ = M_PI / 2.0 - 0.2;
+ wrist_goal_ = -0.55;
+ shooter_velocity_ = 600.0;
intake_goal_ = intake_when_shooting;
} else if (data.IsPressed(kFrontFender)) {
- // Fender shot back
- shoulder_goal_ = 1.45;
- wrist_goal_ = 2.5 + 1.7;
- shooter_velocity_ = 550.0;
+ // Forwards shot, higher
+ shoulder_goal_ = M_PI / 2.0 + 0.1;
+ wrist_goal_ = M_PI + 0.41 + 0.02 + 0.020;
+ if (drivetrain_queue.status.get()) {
+ wrist_goal_ += drivetrain_queue.status->ground_angle;
+ }
+ shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
} else if (data.IsPressed(kExpand) || data.IsPressed(kWinch)) {
// Set the goals to the hanging position so when the actor finishes, we