Merge "Pulled U out of the plant."
diff --git a/y2017/BUILD b/y2017/BUILD
index 7d231dd..249f5f3 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -63,9 +63,24 @@
)
aos_downloader(
+ name = 'download',
+ start_srcs = [
+ ':joystick_reader',
+ ':wpilib_interface',
+ '//aos:prime_start_binaries',
+ '//y2017/control_loops/drivetrain:drivetrain',
+ '//y2017/control_loops/superstructure:superstructure',
+ '//y2017/actors:autonomous_action',
+ ],
+ srcs = [
+ '//aos:prime_binaries',
+ ],
+)
+
+aos_downloader(
name = 'download_stripped',
start_srcs = [
- #':joystick_reader.stripped',
+ ':joystick_reader.stripped',
':wpilib_interface.stripped',
'//aos:prime_start_binaries_stripped',
'//y2017/control_loops/drivetrain:drivetrain.stripped',
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 7fe0693..ea99805 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -103,8 +103,21 @@
break;
case kCompTeamNumber:
- intake->pot_offset = 0.2921 + 0.00039;
- intake->zeroing.measured_absolute_position = 0.045209;
+ intake->pot_offset = 0.26712;
+ intake->zeroing.measured_absolute_position = 0.008913;
+
+ turret->pot_offset = 0;
+ turret->zeroing.measured_absolute_position = 0;
+
+ hood->zeroing.measured_index_position = 0.652898 - 0.488117;
+
+ r->down_error = 0;
+ r->vision_name = "competition";
+ break;
+
+ case kPracticeTeamNumber:
+ intake->pot_offset = 0.2921 + 0.00039 + 0.012236;
+ intake->zeroing.measured_absolute_position = 0.033408;
turret->pot_offset = -5.45 - 0.026111;
turret->zeroing.measured_absolute_position = 0.2429;
@@ -112,22 +125,10 @@
hood->zeroing.measured_index_position = 0.655432 - 0.460505;
r->down_error = 0;
- r->vision_name = "competition";
- break;
-
- case kPracticeTeamNumber:
- intake->pot_offset = 0;
- intake->zeroing.measured_absolute_position = 0;
-
- turret->pot_offset = 0;
- turret->zeroing.measured_absolute_position = 0;
-
- hood->zeroing.measured_index_position = 0.05;
-
- r->down_error = 0;
r->vision_name = "practice";
break;
+
default:
LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
}
diff --git a/y2017/constants.h b/y2017/constants.h
index 49b7afa..e011501 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -74,9 +74,8 @@
control_loops::superstructure::intake::kOutputRatio /
constants::Values::kIntakeEncoderRatio *
kIntakeEncoderCountsPerRevolution;
- static constexpr ::frc971::constants::Range kIntakeRange{
- -0.29878633 * 0.0254, 9.23012063 * 0.0254, (-0.29878633 + 0.125) * 0.0254,
- (9.23012063 - 0.125) * 0.0254};
+ static constexpr ::frc971::constants::Range kIntakeRange{-0.01, 0.240, 0.01,
+ 0.21};
static constexpr double kHoodEncoderCountsPerRevolution = 2048 * 4;
static constexpr double kHoodEncoderRatio = 20.0 / 345.0;
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
index 6b1d59a..1818d62 100755
--- a/y2017/control_loops/python/indexer.py
+++ b/y2017/control_loops/python/indexer.py
@@ -76,7 +76,7 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([.80])
+ self.PlaceControllerPoles([.75])
glog.debug('K: %s', repr(self.K))
glog.debug('Poles are %s',
@@ -154,7 +154,7 @@
q_pos = 0.01
q_vel = 2.0
- q_voltage = 0.4
+ q_voltage = 0.6
if voltage_error_noise is not None:
q_voltage = voltage_error_noise
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index 9c25cb9..f5daa42 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -32,7 +32,7 @@
// The zeroing and operating voltages.
static constexpr double kZeroingVoltage = 2.0;
- static constexpr double kOperatingVoltage = 3.0;
+ static constexpr double kOperatingVoltage = 12.0;
void Iterate(const control_loops::HoodGoal *unsafe_goal,
const ::frc971::IndexPosition *position, double *output,
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
index 78e9528..6186f7f 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.cc
+++ b/y2017/control_loops/superstructure/indexer/indexer.cc
@@ -21,7 +21,6 @@
namespace {
constexpr double kTolerance = 10.0;
-constexpr double kMinStuckVoltage = 3.0;
constexpr chrono::milliseconds kForwardTimeout{500};
constexpr chrono::milliseconds kReverseTimeout{500};
constexpr chrono::milliseconds kReverseMinTimeout{100};
@@ -61,21 +60,15 @@
double IndexerController::voltage() const { return loop_->U(0, 0); }
-double IndexerController::StuckRatio() const {
- double applied_voltage = voltage();
+double IndexerController::StuckVoltage() const {
+ const double applied_voltage = voltage() + loop_->X_hat(2, 0);
if (applied_voltage < 0) {
- applied_voltage = ::std::min(applied_voltage, -kMinStuckVoltage);
+ return +stuck_indexer_X_hat_current_(2, 0) + applied_voltage;
} else {
- applied_voltage = ::std::max(applied_voltage, kMinStuckVoltage);
+ return -stuck_indexer_X_hat_current_(2, 0) - applied_voltage;
}
- // Look at the ratio of the current controller power to the voltage_error
- // term. If our output is dominated by the voltage_error, then we are likely
- // pretty stuck and should try reversing.
- // We don't want to worry about dividing by zero, so keep the applied voltage
- // away from 0 though a min/max.
- return -stuck_indexer_X_hat_current_(2, 0) / applied_voltage;
}
-bool IndexerController::IsStuck() const { return StuckRatio() > 0.6; }
+bool IndexerController::IsStuck() const { return StuckVoltage() > 1.5; }
void IndexerController::Reset() { reset_ = true; }
@@ -139,7 +132,7 @@
status->stuck = IsStuck();
- status->stuck_ratio = StuckRatio();
+ status->stuck_voltage = StuckVoltage();
}
void Indexer::Reset() { indexer_.Reset(); }
diff --git a/y2017/control_loops/superstructure/indexer/indexer.h b/y2017/control_loops/superstructure/indexer/indexer.h
index 5c1cc1b..9d71eed 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.h
+++ b/y2017/control_loops/superstructure/indexer/indexer.h
@@ -39,7 +39,7 @@
// Returns true if the indexer is stuck.
bool IsStuck() const;
- double StuckRatio() const;
+ double StuckVoltage() const;
// Executes the control loop for a cycle.
void Update(bool disabled);
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index b4e6c9b..427dadb 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -69,7 +69,7 @@
// True if the indexer is stuck.
bool stuck;
- float stuck_ratio;
+ float stuck_voltage;
// The state of the indexer state machine.
int32_t state;
diff --git a/y2017/vision/BUILD b/y2017/vision/BUILD
new file mode 100644
index 0000000..c9794fe
--- /dev/null
+++ b/y2017/vision/BUILD
@@ -0,0 +1,36 @@
+load('/aos/build/queues', 'queue_library')
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+load('/tools/build_rules/protobuf', 'proto_cc_library')
+
+package(default_visibility = ["//visibility:public"])
+
+queue_library(
+ name = 'vision_queue',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'vision.q',
+ ],
+)
+
+proto_cc_library(
+ name = 'vision_data',
+ src = 'vision_data.proto',
+)
+
+cc_binary(
+ name = 'target_receiver',
+ srcs = [
+ 'target_receiver.cc',
+ ],
+ visibility = ['//visibility:public'],
+ deps = [
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
+ '//aos/linux_code:init',
+ '//aos/common:time',
+ '//aos/vision/events:udp',
+ ':vision_queue',
+ ':vision_data',
+ '//aos/common:mutex',
+ ],
+)
diff --git a/y2017/vision/target_receiver.cc b/y2017/vision/target_receiver.cc
new file mode 100644
index 0000000..8cabe76
--- /dev/null
+++ b/y2017/vision/target_receiver.cc
@@ -0,0 +1,64 @@
+#include <netdb.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
+#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_data.pb.h"
+
+using aos::monotonic_clock;
+
+namespace y2017 {
+namespace vision {
+
+void ComputeDistanceAngle(const Target &target, double *distance,
+ double *angle) {
+ // TODO: fix this.
+ *distance = target.y();
+ *angle = target.x();
+}
+
+} // namespace vision
+} // namespace y2017
+
+int main() {
+ using namespace y2017::vision;
+ ::aos::events::RXUdpSocket recv(8080);
+ char raw_data[65507];
+
+ while (true) {
+ // TODO(austin): Don't malloc.
+ VisionData target;
+ int size = recv.Recv(raw_data, sizeof(raw_data));
+ monotonic_clock::time_point now = monotonic_clock::now();
+ auto target_time = now -
+ std::chrono::nanoseconds(target.send_timestamp() -
+ target.image_timestamp()) +
+ // It takes a bit to shoot a frame. Push the frame
+ // further back in time.
+ std::chrono::milliseconds(10);
+
+ if (!target.ParseFromArray(raw_data, size)) {
+ continue;
+ }
+
+ auto new_vision_status = vision_status.MakeMessage();
+ new_vision_status->image_valid = target.has_target();
+ if (new_vision_status->image_valid) {
+ new_vision_status->target_time =
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ target_time.time_since_epoch())
+ .count();
+
+ ComputeDistanceAngle(target.target(), &new_vision_status->distance,
+ &new_vision_status->angle);
+ }
+
+ LOG_STRUCT(DEBUG, "vision", *new_vision_status);
+ if (!new_vision_status.Send()) {
+ LOG(ERROR, "Failed to send vision information\n");
+ }
+ }
+}
diff --git a/y2017/vision/vision.q b/y2017/vision/vision.q
new file mode 100644
index 0000000..b3eeee8
--- /dev/null
+++ b/y2017/vision/vision.q
@@ -0,0 +1,14 @@
+package y2017.vision;
+
+message VisionStatus {
+ bool image_valid;
+
+ // Distance to the target in meters.
+ double distance;
+ // The angle in radians of the bottom of the target.
+ double angle;
+
+ // Capture time of the angle using the clock behind monotonic_clock::now().
+ int64_t target_time;
+};
+queue VisionStatus vision_status;
diff --git a/y2017/vision/vision_data.proto b/y2017/vision/vision_data.proto
new file mode 100644
index 0000000..3190686
--- /dev/null
+++ b/y2017/vision/vision_data.proto
@@ -0,0 +1,19 @@
+syntax = "proto2";
+
+package y2017.vision;
+
+// Represents a target found by the vision processing code.
+// X is an estimate of the center of the target.
+// Y is an estimate of the top of the bottom retroreflective tape.
+message Target {
+ optional double x = 1;
+ optional double y = 2;
+}
+
+// Represents the best target in the image if there is such a target
+// along with timing information.
+message VisionData {
+ optional int64 image_timestamp = 1;
+ optional int64 send_timestamp = 2;
+ optional Target target = 3;
+}