Merge "Make target_sender send data out the UART"
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 168f81d..d097ec8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -337,7 +337,8 @@
dt_closedloop_.Update(output != NULL && controller_type == 1);
- dt_spline_.Update(output != NULL && controller_type == 2);
+ dt_spline_.Update(output != NULL && controller_type == 2,
+ xytheta_state_.block<5, 1>(0, 0));
switch (controller_type) {
case 0:
@@ -389,6 +390,7 @@
dt_openloop_.PopulateStatus(status);
dt_closedloop_.PopulateStatus(status);
+ dt_spline_.PopulateStatus(status);
}
double left_voltage = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 88547d5..0f57bf9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -111,6 +111,15 @@
return angular;
}
+ Eigen::Matrix<Scalar, 2, 2> Tlr_to_la() const {
+ return (::Eigen::Matrix<Scalar, 2, 2>() << 0.5, 0.5,
+ -1.0 / (2 * robot_radius), 1.0 / (2 * robot_radius)).finished();
+ }
+
+ Eigen::Matrix<Scalar, 2, 2> Tla_to_lr() const {
+ return Tlr_to_la().inverse();
+ }
+
// Converts the linear and angular position, velocity to the top 4 states of
// the robot state.
Eigen::Matrix<Scalar, 4, 1> AngularLinearToLeftRight(
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 17da3ab..6c16667 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -13,6 +13,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/queues/gyro.q.h"
#include "y2016/constants.h"
@@ -108,7 +109,15 @@
".frc971.control_loops.drivetrain_queue.position",
".frc971.control_loops.drivetrain_queue.output",
".frc971.control_loops.drivetrain_queue.status"),
- gyro_reading_(::frc971::sensors::gyro_reading.name()) {
+ gyro_reading_(::frc971::sensors::gyro_reading.name()),
+ velocity_drivetrain_(::std::unique_ptr<StateFeedbackLoop<
+ 2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>(
+ new StateFeedbackLoop<2, 2, 2, double,
+ StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>(
+ GetDrivetrainConfig()
+ .make_hybrid_drivetrain_velocity_loop()))) {
Reinitialize();
last_U_.setZero();
}
@@ -188,6 +197,16 @@
U(0, 0) += drivetrain_plant_->left_voltage_offset();
U(1, 0) += drivetrain_plant_->right_voltage_offset();
drivetrain_plant_->Update(U);
+ double dt_float =
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ GetDrivetrainConfig().dt).count();
+ state = RungeKuttaU(
+ [this](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return ContinuousDynamics(velocity_drivetrain_->plant(),
+ GetDrivetrainConfig().Tlr_to_la(), X, U);
+ },
+ state, U, dt_float);
}
void set_left_voltage_offset(double left_voltage_offset) {
@@ -199,10 +218,18 @@
::std::unique_ptr<DrivetrainPlant> drivetrain_plant_;
+ ::Eigen::Matrix<double, 2, 1> GetPosition() const {
+ return state.block<2,1>(0,0);
+ }
+
private:
::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
::aos::Queue<::frc971::sensors::GyroReading> gyro_reading_;
+ ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+ ::std::unique_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>> velocity_drivetrain_;
double last_left_position_;
double last_right_position_;
@@ -259,6 +286,12 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
+ void VerifyNearPosition(::Eigen::Matrix<double, 2, 1> expected) {
+ auto actual = drivetrain_motor_plant_.GetPosition();
+ EXPECT_NEAR(actual(0), expected(0), 1e-2);
+ EXPECT_NEAR(actual(1), expected(1), 1e-2);
+ }
+
virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
};
@@ -526,6 +559,135 @@
VerifyNearGoal();
}
+// Tests that simple spline converges on a goal.
+TEST_F(DrivetrainTest, SplineSimple) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that simple spline converges when it doesn't start where it thinks.
+TEST_F(DrivetrainTest, SplineOffset) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.5, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that simple spline converges when it starts to the side of where it
+// thinks.
+TEST_F(DrivetrainTest, SplineSideOffset) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.5, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that a multispline converges on a goal.
+TEST_F(DrivetrainTest, MultiSpline) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 2;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+
+ RunForTime(chrono::milliseconds(4000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+}
+
+// Tests that several splines converges on a goal.
+TEST_F(DrivetrainTest, SequentialSplines) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_spline_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ second_spline_goal->controller_type = 2;
+ second_spline_goal->spline.spline_idx = 2;
+ second_spline_goal->spline.spline_count = 1;
+ second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ second_spline_goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ second_start_goal->controller_type = 2;
+ second_start_goal->spline_handle = 2;
+ second_start_goal.Send();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+}
+
::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
double x2_min, double x2_max) {
Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0d6b6ae..b45f218 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,5 +1,7 @@
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+#include <iostream>
+
#include "Eigen/Dense"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -12,7 +14,17 @@
namespace drivetrain {
SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
- : dt_config_(dt_config) {}
+ : dt_config_(dt_config),
+ current_state_(::Eigen::Matrix<double, 2, 1>::Zero()) {}
+
+void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
+ bool output_was_capped = ::std::abs((*U)(0, 0)) > 12.0 ||
+ ::std::abs((*U)(1, 0)) > 12.0;
+
+ if (output_was_capped) {
+ *U *= 12.0 / U->lpNorm<Eigen::Infinity>();
+ }
+}
// TODO(alex): put in another thread to avoid malloc in RT.
void SplineDrivetrain::SetGoal(
@@ -65,17 +77,36 @@
current_trajectory_->Plan();
current_xva_ = current_trajectory_->FFAcceleration(0);
current_xva_(1) = 0.0;
+ current_state_ = ::Eigen::Matrix<double, 2, 1>::Zero();
}
}
-void SplineDrivetrain::Update(bool enable) {
- if (enable && current_trajectory_ &&
- !current_trajectory_->is_at_end(current_state_)) {
+// TODO(alex): Hold position when done following the spline.
+// TODO(Austin): Compensate for voltage error.
+void SplineDrivetrain::Update(bool enable,
+ const ::Eigen::Matrix<double, 5, 1> &state) {
+ enable_ = enable;
+ if (enable && current_trajectory_) {
+ ::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
+ if (!current_trajectory_->is_at_end(current_state_)) {
+ // TODO(alex): It takes about a cycle for the outputs to propagate to the
+ // motors. Consider delaying the output by a cycle.
+ U_ff = current_trajectory_->FFVoltage(current_xva_(0));
+ }
+ ::Eigen::Matrix<double, 2, 5> K =
+ current_trajectory_->KForState(state, dt_config_.dt, Q, R);
+ ::Eigen::Matrix<double, 5, 1> goal_state =
+ current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+ ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+ ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+ next_U_ = U_ff + U_fb;
+ uncapped_U_ = next_U_;
+ ScaleCapU(&next_U_);
+
next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, ¤t_state_);
}
}
-// TODO(alex): Handle drift.
void SplineDrivetrain::SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output) {
if (!output) {
@@ -86,16 +117,22 @@
}
if (current_spline_handle_ == current_spline_idx_) {
if (!current_trajectory_->is_at_end(current_state_)) {
- double current_distance = current_xva_(0);
- ::Eigen::Matrix<double, 2, 1> FFVoltage =
- current_trajectory_->FFVoltage(current_distance);
- output->left_voltage = FFVoltage(0);
- output->right_voltage = FFVoltage(1);
+ output->left_voltage = next_U_(0);
+ output->right_voltage = next_U_(1);
current_xva_ = next_xva_;
}
}
}
+void SplineDrivetrain::PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+ if (status && enable_) {
+ status->uncapped_left_voltage = uncapped_U_(0);
+ status->uncapped_right_voltage = uncapped_U_(1);
+ status->robot_speed = current_xva_(1);
+ }
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 4bb7ac9..a065167 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -19,7 +19,7 @@
void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
- void Update(bool enabled);
+ void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
void SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output);
@@ -27,6 +27,8 @@
void PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const;
private:
+ void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
+
const DrivetrainConfig<double> dt_config_;
uint32_t current_spline_handle_; // Current spline told to excecute.
@@ -35,6 +37,25 @@
::std::unique_ptr<Trajectory> current_trajectory_;
::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
::Eigen::Matrix<double, 2, 1> current_state_;
+ ::Eigen::Matrix<double, 2, 1> next_U_;
+ ::Eigen::Matrix<double, 2, 1> uncapped_U_;
+
+ bool enable_;
+
+ // TODO(alex): pull this out of dt_config.
+ const ::Eigen::DiagonalMatrix<double, 5> Q =
+ (::Eigen::DiagonalMatrix<double, 5>().diagonal()
+ << 1.0 / ::std::pow(0.05, 2),
+ 1.0 / ::std::pow(0.05, 2), 1.0 / ::std::pow(0.2, 2),
+ 1.0 / ::std::pow(0.5, 2), 1.0 / ::std::pow(0.5, 2))
+ .finished()
+ .asDiagonal();
+ const ::Eigen::DiagonalMatrix<double, 2> R =
+ (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 1.0 / ::std::pow(12.0, 2),
+ 1.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal();
};
} // namespace drivetrain
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 26b2cb2..e59286f 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -26,9 +26,8 @@
return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
-def mToPx(i):
- return (i * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
-
+def mToPx(m):
+ return (m*PIXELS_ON_SCREEN/WIDTH_OF_FIELD_IN_METERS)
def px(cr):
return OverrideMatrix(cr, identity)
@@ -229,7 +228,138 @@
(points[index - 1][1] - point[1])**2)
return distance
- # Handle the expose-event by updating the Window and drawing
+ def draw_HAB(self, cr):
+ print("WENT IN")
+ # BASE Constants
+ X_BASE = -450+mToPx(2.41568)
+ Y_BASE = -150+mToPx(4.129151)
+
+ BACKWALL_X = -450
+
+ # HAB Levels 2 and 3 called in variables backhab
+
+ WIDTH_BACKHAB = mToPx(1.2192)
+
+ Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+ BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+ BACKHAB_LV3_LENGTH = mToPx(1.2192)
+ Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+ Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX -BACKHAB_LV2_LENGTH
+
+ # HAB LEVEL 1
+ X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+ WIDTH_LV1_BOX = mToPx(0.90805)
+ LENGTH_LV1_BOX = mToPx(1.6256)
+
+ Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+ # Ramp off Level 1
+ X_RAMP = X_LV1_BOX
+
+ Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+ WIDTH_TOP_RAMP = mToPx(1.20015)
+ LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+ X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+ Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+ LENGTH_MIDDLE_RAMP = 2*LENGTH_LV1_BOX
+ WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+ Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+ # Side Bars to Hold in balls
+ X_BARS = BACKWALL_X
+ WIDTH_BARS = WIDTH_BACKHAB
+ LENGTH_BARS = mToPx(0.574675)
+
+ Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+ Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+ set_color(cr, palette["BLACK"])
+ cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV3_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX,
+ LENGTH_LV1_BOX)
+ cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+ LENGTH_MIDDLE_RAMP)
+ cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.stroke()
+ #draw_px_x(cr, BACKWALL_X, 0, 10) # Midline Point
+ #draw_px_x(cr, X_BASE, Y_BASE, 10) # Bases
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+ cr.stroke()
+
+ def draw_rockets(self, cr):
+ # BASE Constants
+ X_BASE = -450+mToPx(2.41568)
+ Y_BASE = -150+mToPx(4.129151)
+
+ # Top Rocket
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ # Bottom Rocket
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ cr.stroke()
+
+ def draw_cargo_ship(self, cr):
+ # BASE Constants
+ X_BASE = -450+mToPx(2.41568)
+ Y_BASE = -150+mToPx(4.129151)
+
+ cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
+ mToPx(2.43205), mToPx(1.41605))
+
+ cr.stroke()
+
+ def draw_field_elements(self, cr):
+ self.draw_HAB(cr)
+ self.draw_rockets(cr)
+ self.draw_cargo_ship(cr)
+
def handle_draw(self, cr):
# print(self.new_point)
# print("SELF.POINT_SELECTED: " + str(self.point_selected))
@@ -248,7 +378,7 @@
self.extents_y_max - self.extents_y_min)
cr.fill()
- #Drawing the switch and scale in the field
+ #Drawing the field
cr.move_to(0, 50)
cr.show_text('Press "e" to export')
cr.show_text('Press "i" to import')
@@ -261,6 +391,8 @@
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
+ self.draw_field_elements(cr)
+
y = 0
# update all the things
diff --git a/tools/bazel b/tools/bazel
index dd5d9a5..a691066 100755
--- a/tools/bazel
+++ b/tools/bazel
@@ -26,7 +26,7 @@
readonly VERSION="0.19.0rc4-201810201638+ac88041"
-readonly DOWNLOAD_DIR="$(dirname "${BASH_SOURCE[0]}")/../bazel-downloads"
+readonly DOWNLOAD_DIR="${HOME}/.cache/bazel"
# Directory to unpack bazel into. This must change whenever bazel changes.
readonly VERSION_DIR="${DOWNLOAD_DIR}/${VERSION}-v1"
readonly VERSION_BAZEL="${VERSION_DIR}/usr/bin/bazel"
diff --git a/tools/ci/run-tests.sh b/tools/ci/run-tests.sh
index 93e5d09..4688378 100755
--- a/tools/ci/run-tests.sh
+++ b/tools/ci/run-tests.sh
@@ -6,7 +6,8 @@
# Include --config=eigen to enable Eigen assertions so that we catch potential
# bugs with Eigen.
-bazel test -c opt --config=eigen --curses=no --color=no ${TARGETS}
-bazel build -c opt --curses=no --color=no ${TARGETS} --cpu=roborio
-bazel build --curses=no --color=no ${TARGETS} --cpu=armhf-debian
-bazel build -c opt --curses=no --color=no //motors/... //y2019/jevois/... --cpu=cortex-m4f
+bazel test -c opt --stamp=no --config=eigen --curses=no --color=no ${TARGETS}
+bazel build -c opt --stamp=no --curses=no --color=no ${TARGETS} --cpu=roborio
+bazel build --stamp=no --curses=no --color=no ${TARGETS} --cpu=armhf-debian
+bazel build -c opt --stamp=no --curses=no --color=no \
+ //motors/... //y2019/jevois/... --cpu=cortex-m4f
diff --git a/y2019/BUILD b/y2019/BUILD
index ab4a842..14d0d6f 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -24,7 +24,9 @@
"//aos/mutex",
"//aos/network:team_number",
"//frc971:constants",
+ "//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//y2019/control_loops/drivetrain:camera",
"//y2019/control_loops/drivetrain:polydrivetrain_plants",
"//y2019/control_loops/superstructure/elevator:elevator_plants",
"//y2019/control_loops/superstructure/intake:intake_plants",
@@ -74,6 +76,30 @@
],
)
+cc_library(
+ name = "joystick_angle",
+ srcs = [
+ "joystick_angle.cc",
+ ],
+ hdrs = [
+ "joystick_angle.h",
+ ],
+ deps = [
+ "//aos/input:drivetrain_input",
+ ],
+)
+
+cc_test(
+ name = "joystick_angle_test",
+ srcs = [
+ "joystick_angle_test.cc",
+ ],
+ deps = [
+ ":joystick_angle",
+ "//aos/testing:googletest",
+ ],
+)
+
cc_binary(
name = "joystick_reader",
srcs = [
diff --git a/y2019/constants.cc b/y2019/constants.cc
index b53fb99..791a6c4 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -30,6 +30,16 @@
const uint16_t kPracticeTeamNumber = 9971;
const uint16_t kCodingRobotTeamNumber = 7971;
+constexpr double FeetToMeters(double ft) {
+ return 0.3048 * ft;
+}
+constexpr double InchToMeters(double in) {
+ return 0.0254 * in;
+}
+constexpr double DegToRad(double deg) {
+ return deg * M_PI / 180.0;
+}
+
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
Values::PotAndAbsConstants *const elevator = &r->elevator;
@@ -99,7 +109,7 @@
stilts_params->zeroing_voltage = 3.0;
stilts_params->operating_voltage = 12.0;
stilts_params->zeroing_profile_params = {0.1, 0.5};
- stilts_params->default_profile_params = {0.5, 0.5};
+ stilts_params->default_profile_params = {0.1, 0.5};
stilts_params->range = Values::kStiltsRange();
stilts_params->make_integral_loop =
&control_loops::superstructure::stilts::MakeIntegralStiltsLoop;
@@ -210,5 +220,154 @@
return *values[team_number];
}
+constexpr size_t Field::kNumTargets;
+constexpr size_t Field::kNumObstacles;
+
+Field::Field() {
+ // TODO(james): These values need to re-verified. I got them by skimming the
+ // manual and they all seem to be pretty much correct.
+ //
+ // Note: Per //frc971/control_loops/pose.h, coordinate system is:
+ // -In meters
+ // -Origin at center of our driver's station wall
+ // -Positive X-axis pointing straight out from driver's station
+ // -Positive Y-axis pointing straight left from the driver's perspective
+ // -Positive Z-axis is straight up.
+ // -The angle of the target is such that the angle is the angle you would
+ // need to be facing to see it straight-on. I.e., if the target angle is
+ // pi / 2.0, then you would be able to see it face on by facing straight
+ // left from the driver's point of view (if you were standing in the right
+ // spot).
+ constexpr double kCenterFieldX = FeetToMeters(27.0) + InchToMeters(1.125);
+
+ constexpr double kFarSideCargoBayX =
+ kCenterFieldX - InchToMeters(20.875);
+ constexpr double kMidSideCargoBayX = kFarSideCargoBayX - InchToMeters(21.75);
+ constexpr double kNearSideCargoBayX = kMidSideCargoBayX - InchToMeters(21.75);
+ constexpr double kSideCargoBayY = InchToMeters(24 + 3 + 0.875);
+ constexpr double kSideCargoBayTheta = -M_PI_2;
+
+ constexpr double kFaceCargoBayX =
+ kCenterFieldX - InchToMeters(7 * 12 + 11.75 + 9);
+ constexpr double kFaceCargoBayY = InchToMeters(10.875);
+ constexpr double kFaceCargoBayTheta = 0.0;
+
+ constexpr double kRocketX = kCenterFieldX - FeetToMeters(8);
+ constexpr double kRocketY = InchToMeters((26 * 12 + 10.5) / 2.0);
+
+ constexpr double kRocketPortX = kRocketX;
+ constexpr double kRocketPortY = kRocketY - 0.70;
+ constexpr double kRocketPortTheta = M_PI_2;
+
+ // Half of portal + guess at width * cos(61.5 deg)
+ const double kRocketHatchXOffset = InchToMeters(14.634);
+ const double kRocketHatchY = kRocketPortY + InchToMeters(9.326);
+ const double kRocketNearX = kRocketX - kRocketHatchXOffset;
+ const double kRocketFarX = kRocketX + kRocketHatchXOffset;
+ constexpr double kRocketNearTheta = DegToRad(28.5);
+ constexpr double kRocketFarTheta = M_PI - kRocketNearTheta;
+
+ constexpr double kHpSlotY = InchToMeters((26 * 12 + 10.5) / 2.0 - 25.9);
+ constexpr double kHpSlotTheta = M_PI;
+
+ constexpr double kNormalZ = 0.80;
+ constexpr double kPortZ = 0.99;
+
+ const Pose far_side_cargo_bay({kFarSideCargoBayX, kSideCargoBayY, kNormalZ},
+ kSideCargoBayTheta);
+ const Pose mid_side_cargo_bay({kMidSideCargoBayX, kSideCargoBayY, kNormalZ},
+ kSideCargoBayTheta);
+ const Pose near_side_cargo_bay({kNearSideCargoBayX, kSideCargoBayY, kNormalZ},
+ kSideCargoBayTheta);
+
+ const Pose face_cargo_bay({kFaceCargoBayX, kFaceCargoBayY, kNormalZ},
+ kFaceCargoBayTheta);
+
+ const Pose rocket_port({kRocketPortX, kRocketPortY, kPortZ},
+ kRocketPortTheta);
+
+ const Pose rocket_near({kRocketNearX, kRocketHatchY, kNormalZ},
+ kRocketNearTheta);
+ const Pose rocket_far({kRocketFarX, kRocketHatchY, kNormalZ},
+ kRocketFarTheta);
+
+ const Pose hp_slot({0.0, kHpSlotY, kNormalZ}, kHpSlotTheta);
+
+ const ::std::array<Pose, 8> quarter_field_targets{
+ {far_side_cargo_bay, mid_side_cargo_bay, near_side_cargo_bay,
+ face_cargo_bay, rocket_port, rocket_near, rocket_far, hp_slot}};
+
+ // Mirror across center field mid-line (short field axis):
+ ::std::array<Pose, 16> half_field_targets;
+ ::std::copy(quarter_field_targets.begin(), quarter_field_targets.end(),
+ half_field_targets.begin());
+ for (int ii = 0; ii < 8; ++ii) {
+ const int jj = ii + 8;
+ half_field_targets[jj] = quarter_field_targets[ii];
+ half_field_targets[jj].mutable_pos()->x() =
+ 2.0 * kCenterFieldX - half_field_targets[jj].rel_pos().x();
+ half_field_targets[jj].set_theta(
+ aos::math::NormalizeAngle(M_PI - half_field_targets[jj].rel_theta()));
+ }
+
+ ::std::array<Pose, 32> target_poses_;
+
+ // Mirror across x-axis (long field axis):
+ ::std::copy(half_field_targets.begin(), half_field_targets.end(),
+ target_poses_.begin());
+ for (int ii = 0; ii < 16; ++ii) {
+ const int jj = ii + 16;
+ target_poses_[jj] = half_field_targets[ii];
+ target_poses_[jj].mutable_pos()->y() *= -1;
+ target_poses_[jj].set_theta(-target_poses_[jj].rel_theta());
+ }
+ for (int ii = 0; ii < 32; ++ii) {
+ targets_[ii] = {target_poses_[ii]};
+ }
+
+ // Define rocket obstacles as just being a single line that should block any
+ // cameras trying to see through the rocket up and down the field.
+ // This line is parallel to the driver's station wall and extends behind
+ // the portal.
+ Obstacle rocket_obstacle({{kRocketPortX, kRocketY, 0.0}, 0.0},
+ {{kRocketPortX, kRocketPortY + 0.01, 0.0}, 0.0});
+ // First, we mirror rocket obstacles across x-axis:
+ Obstacle rocket_obstacle2({{kRocketPortX, -kRocketY, 0.0}, 0.0},
+ {{kRocketPortX, -kRocketPortY - 0.01, 0.0}, 0.0});
+
+ // Define an obstacle for the Hab that extends striaght out a few feet from
+ // the driver's station wall.
+ // TODO(james): Does this actually block our view?
+ const double kHabL3X = FeetToMeters(4.0);
+ Obstacle hab_obstacle({}, {{kHabL3X, 0.0, 0.0}, 0.0});
+ ::std::array<Obstacle, 3> half_obstacles{
+ {rocket_obstacle, rocket_obstacle2, hab_obstacle}};
+ ::std::copy(half_obstacles.begin(), half_obstacles.end(), obstacles_.begin());
+
+ // Next, we mirror across the mid-line (short axis) to duplicate the
+ // rockets and hab to opposite side of the field.
+ for (int ii = 0; ii < 3; ++ii) {
+ const int jj = ii + 3;
+ obstacles_[jj] = half_obstacles[ii];
+ obstacles_[jj].mutable_pose1()->mutable_pos()->x() =
+ 2.0 * kCenterFieldX - obstacles_[jj].mutable_pose1()->rel_pos().x();
+ obstacles_[jj].mutable_pose2()->mutable_pos()->x() =
+ 2.0 * kCenterFieldX - obstacles_[jj].mutable_pose2()->rel_pos().x();
+ }
+
+ // Finally, define a rectangular cargo ship.
+ const double kCargoCornerX = kFaceCargoBayX + 0.1;
+ const double kCargoCornerY = kSideCargoBayY - 0.1;
+ ::std::array<Pose, 4> cargo_corners{
+ {{{kCargoCornerX, kCargoCornerY, 0.0}, 0.0},
+ {{kCargoCornerX, -kCargoCornerY, 0.0}, 0.0},
+ {{2.0 * kCenterFieldX - kCargoCornerX, -kCargoCornerY, 0.0}, 0.0},
+ {{2.0 * kCenterFieldX - kCargoCornerX, kCargoCornerY, 0.0}, 0.0}}};
+ for (int ii = 6; ii < 10; ++ii) {
+ obstacles_[ii] = Obstacle(cargo_corners[ii % cargo_corners.size()],
+ cargo_corners[(ii + 1) % cargo_corners.size()]);
+ }
+}
+
} // namespace constants
} // namespace y2019
diff --git a/y2019/constants.h b/y2019/constants.h
index 46d28f1..24d8b92 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -1,6 +1,7 @@
#ifndef Y2019_CONSTANTS_H_
#define Y2019_CONSTANTS_H_
+#include <array>
#include <math.h>
#include <stdint.h>
@@ -11,6 +12,8 @@
#include "y2019/control_loops/superstructure/intake/intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
+#include "y2019/control_loops/drivetrain/camera.h"
+#include "frc971/control_loops/pose.h"
namespace y2019 {
namespace constants {
@@ -25,6 +28,33 @@
//
// All ratios are from the encoder shaft to the output units.
+
+class Field {
+ public:
+ typedef ::frc971::control_loops::TypedPose<double> Pose;
+ typedef ::y2019::control_loops::TypedTarget<double> Target;
+ typedef ::frc971::control_loops::TypedLineSegment<double> Obstacle;
+
+ static constexpr size_t kNumTargets = 32;
+ static constexpr size_t kNumObstacles = 10;
+
+ Field();
+
+ ::std::array<Target, kNumTargets> targets() const { return targets_; }
+ ::std::array<Obstacle, kNumObstacles> obstacles() const { return obstacles_; }
+
+ private:
+ // All target locations are defined as being at the center of the target,
+ // except for the height, for which we use the top of the target.
+ ::std::array<Target, kNumTargets> targets_;
+ // Obstacle locations are approximate, as we are just trying to roughly
+ // approximate what will block our view when on the field.
+ // If anything, we should err on the side of making obstacles too small so
+ // that if there is any error in our position, we don't assume that it must
+ // be hidden behind a target when it really is not.
+ ::std::array<Obstacle, kNumObstacles> obstacles_;
+};
+
struct Values {
static const int kZeroingSampleSize = 200;
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 959e839..38c73f0 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -84,6 +84,7 @@
cc_library(
name = "camera",
srcs = ["camera.h"],
+ visibility = ["//y2019:__pkg__"],
deps = [
"//aos/containers:sized_array",
"//frc971/control_loops:pose",
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 5edfce3..187379b 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -8,16 +8,33 @@
namespace control_loops {
namespace superstructure {
-void suction_cups(
- const SuperstructureQueue::Goal *unsafe_goal,
- SuperstructureQueue::Output *output) {
- const double on_voltage = 12.0;
+void Superstructure::HandleSuction(const SuctionGoal *unsafe_goal,
+ float suction_pressure,
+ SuperstructureQueue::Output *output,
+ bool *has_piece) {
+ constexpr double kPumpVoltage = 12.0;
+ constexpr double kPumpHasPieceVoltage = 8.0;
- if(unsafe_goal && output) {
- if(unsafe_goal->suction.top || unsafe_goal->suction.bottom) {
- output->pump_voltage = on_voltage;
+ // TODO(austin): Low pass filter on pressure.
+ *has_piece = suction_pressure < 0.70;
+
+ if (unsafe_goal && output) {
+ const bool evacuate = unsafe_goal->top || unsafe_goal->bottom;
+ if (evacuate) {
+ vacuum_count_ = 200;
}
+ // TODO(austin): High speed pump a bit longer after we detect we have the
+ // game piece.
+ // Once the vacuum evacuates, the pump speeds up because there is no
+ // resistance. So, we want to turn it down to save the pump from
+ // overheating.
+ output->pump_voltage =
+ (vacuum_count_ > 0) ? (*has_piece ? kPumpHasPieceVoltage : kPumpVoltage)
+ : 0.0;
+ output->intake_suction_top = unsafe_goal->top;
+ output->intake_suction_bottom = unsafe_goal->bottom;
}
+ vacuum_count_ = ::std::max(0, vacuum_count_ - 1);
}
Superstructure::Superstructure(::aos::EventLoop *event_loop,
@@ -60,6 +77,9 @@
output != nullptr ? &(output->stilts_voltage) : nullptr,
&(status->stilts));
+ HandleSuction(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
+ position->suction_pressure, output, &(status->has_piece));
+
status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
status->intake.zeroed && status->stilts.zeroed;
@@ -67,10 +87,8 @@
status->intake.estopped || status->stilts.estopped;
if (output) {
- if (status->intake.position > kMinIntakeAngleForRollers) {
- output->intake_roller_voltage =
- (unsafe_goal != nullptr) ? unsafe_goal->roller_voltage : 0.0;
-
+ if (unsafe_goal && status->intake.position > kMinIntakeAngleForRollers) {
+ output->intake_roller_voltage = unsafe_goal->roller_voltage;
} else {
output->intake_roller_voltage = 0.0;
}
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index c3e53f2..9879e17 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -40,12 +40,16 @@
SuperstructureQueue::Status *status) override;
private:
+ void HandleSuction(const SuctionGoal *unsafe_goal, float suction_pressure,
+ SuperstructureQueue::Output *output, bool *has_piece);
+
PotAndAbsoluteEncoderSubsystem elevator_;
PotAndAbsoluteEncoderSubsystem wrist_;
AbsoluteEncoderSubsystem intake_;
PotAndAbsoluteEncoderSubsystem stilts_;
CollisionAvoidance collision_avoidance_;
+ int vacuum_count_ = 0;
static constexpr double kMinIntakeAngleForRollers = -0.7;
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index a71f81a..4176057 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -53,9 +53,9 @@
};
message Position {
- // Input from pressure sensor in psi
- // 0 = current atmospheric pressure, negative = suction.
- double suction_pressure;
+ // Input from pressure sensor in bar
+ // 1 = 1 atm, 0 = full vacuum
+ float suction_pressure;
// Position of the elevator, 0 at lowest position, positive when up.
.frc971.PotAndAbsolutePosition elevator;
diff --git a/y2019/joystick_angle.cc b/y2019/joystick_angle.cc
new file mode 100644
index 0000000..1923c53
--- /dev/null
+++ b/y2019/joystick_angle.cc
@@ -0,0 +1,36 @@
+#include "y2019/joystick_angle.h"
+
+namespace y2019 {
+namespace input {
+namespace joysticks {
+
+JoystickAngle GetJoystickPosition(const JoystickAxis &x_axis,
+ const JoystickAxis &y_axis,
+ const Data &data) {
+ return GetJoystickPosition(data.GetAxis(x_axis), data.GetAxis(y_axis));
+}
+
+JoystickAngle GetJoystickPosition(float x_axis, float y_axis) {
+ if (x_axis > kJoystickRight) {
+ if (y_axis < kJoystickDown) {
+ return JoystickAngle::kUpperRight;
+ } else if (y_axis > kJoystickUp) {
+ return JoystickAngle::kLowerRight;
+ } else {
+ return JoystickAngle::kMiddleRight;
+ }
+ } else if (x_axis < kJoystickLeft) {
+ if (y_axis < kJoystickDown) {
+ return JoystickAngle::kUpperLeft;
+ } else if (y_axis > kJoystickUp) {
+ return JoystickAngle::kLowerLeft;
+ } else {
+ return JoystickAngle::kMiddleLeft;
+ }
+ }
+ return JoystickAngle::kDefault;
+}
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2019
diff --git a/y2019/joystick_angle.h b/y2019/joystick_angle.h
new file mode 100644
index 0000000..a2c7e36
--- /dev/null
+++ b/y2019/joystick_angle.h
@@ -0,0 +1,37 @@
+#ifndef Y2019_JOYSTICK_ANGLE_H_
+#define Y2019_JOYSTICK_ANGLE_H_
+
+#include "aos/input/driver_station_data.h"
+
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::Data;
+
+namespace y2019 {
+namespace input {
+namespace joysticks {
+namespace {
+constexpr double kJoystickLeft = -0.5;
+constexpr double kJoystickRight = 0.5;
+constexpr double kJoystickUp = 0.5;
+constexpr double kJoystickDown = -0.5;
+}
+
+enum class JoystickAngle {
+ kDefault,
+ kUpperRight,
+ kMiddleRight,
+ kLowerRight,
+ kUpperLeft,
+ kMiddleLeft,
+ kLowerLeft
+};
+
+JoystickAngle GetJoystickPosition(const JoystickAxis &x_axis,
+ const JoystickAxis &y_axis, const Data &data);
+JoystickAngle GetJoystickPosition(float x_axis, float y_axis);
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2019
+
+#endif // Y2019_JOYSTICK_ANGLE_H_
diff --git a/y2019/joystick_angle_test.cc b/y2019/joystick_angle_test.cc
new file mode 100644
index 0000000..c352359
--- /dev/null
+++ b/y2019/joystick_angle_test.cc
@@ -0,0 +1,18 @@
+#include "y2019/joystick_angle.h"
+#include <iostream>
+#include "aos/input/driver_station_data.h"
+#include "gtest/gtest.h"
+
+using y2019::input::joysticks::JoystickAngle;
+using y2019::input::joysticks::GetJoystickPosition;
+
+TEST(JoystickAngleTest, JoystickAngleTest) {
+ EXPECT_EQ(JoystickAngle::kUpperRight, GetJoystickPosition(0.75, -0.75));
+ EXPECT_EQ(JoystickAngle::kMiddleRight, GetJoystickPosition(0.75, 0));
+ EXPECT_EQ(JoystickAngle::kLowerRight, GetJoystickPosition(0.75, 0.75));
+ EXPECT_EQ(JoystickAngle::kUpperLeft, GetJoystickPosition(-0.75, -0.75));
+ EXPECT_EQ(JoystickAngle::kMiddleLeft, GetJoystickPosition(-0.75, 0));
+ EXPECT_EQ(JoystickAngle::kLowerLeft, GetJoystickPosition(-0.75, 0.75));
+
+ EXPECT_EQ(JoystickAngle::kDefault, GetJoystickPosition(0, 0));
+}
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index f718c3d..17cc45f 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -30,14 +30,28 @@
namespace joysticks {
// TODO(sabina): update button locations when the board is done
-const ButtonLocation kElevatorUp(4, 4);
const ButtonLocation kIntakeOut(3, 3);
const ButtonLocation kElevatorDown(0, 0);
-const ButtonLocation kElevator1(4, 1);
-const ButtonLocation kElevator2(4, 11);
-const ButtonLocation kElevator3(4, 9);
-const ButtonLocation kElevator4(4, 7);
-const ButtonLocation kElevator5(4, 5);
+const ButtonLocation kElevatorFront1(4, 1);
+const ButtonLocation kElevatorFront2(4, 11);
+const ButtonLocation kElevatorFront3(4, 9);
+const ButtonLocation kElevatorFront4(4, 7);
+const ButtonLocation kElevatorFront5(4, 5);
+
+const ButtonLocation kElevatorBack1(3, 14);
+const ButtonLocation kElevatorBack2(4, 12);
+const ButtonLocation kElevatorBack3(4, 10);
+const ButtonLocation kElevatorBack4(4, 8);
+const ButtonLocation kElevatorBack5(4, 6);
+
+const ButtonLocation kElevatorIntaking(3, 4);
+const ButtonLocation kElevatorIntakingUp(3, 6);
+const ButtonLocation kRelease(4, 4);
+
+const ButtonLocation kSuctionBall(3, 13);
+const ButtonLocation kSuctionHatch(3, 12);
+const ButtonLocation kDeployStilt(3, 8);
+const ButtonLocation kFallOver(3, 9);
const ButtonLocation kDiskLoad(0, 0);
const ButtonLocation kDiskRocketMiddle(0, 0);
@@ -54,11 +68,11 @@
const ButtonLocation kCargoSuction(0, 0);
const ButtonLocation kDiskSuction(0, 0);
const ButtonLocation kSuctionOut(0, 0);
-const ButtonLocation kDeployStilt(3, 8);
const ButtonLocation kRetractStilt(0, 0);
const ButtonLocation kBackwards(0, 0);
-const ButtonLocation kWristDown(3, 1);
+const ButtonLocation kWristBackwards(3, 10);
+const ButtonLocation kWristForwards(3, 7);
class Reader : public ::aos::input::ActionJoystickInput {
public:
@@ -146,6 +160,7 @@
// TODO(sabina): decide if we should really have disk suction as its own
// button
+ /*
if (data.IsPressed(kCargoSuction)) {
new_superstructure_goal->suction.top = false;
new_superstructure_goal->suction.bottom = true;
@@ -156,39 +171,104 @@
new_superstructure_goal->suction.top = true;
new_superstructure_goal->suction.bottom = true;
}
+ */
+
+ if (data.IsPressed(kSuctionBall)) {
+ top_ = false;
+ bottom_ = true;
+ } else if (data.IsPressed(kSuctionHatch)) {
+ top_ = true;
+ bottom_ = true;
+ } else if (data.IsPressed(kRelease) ||
+ !superstructure_queue.status->has_piece) {
+ top_ = false;
+ bottom_ = false;
+ }
// TODO(sabina): max height please?
if (data.IsPressed(kDeployStilt)) {
- new_superstructure_goal->stilts.unsafe_goal = 0.3;
+ new_superstructure_goal->stilts.unsafe_goal = 0.50;
+ } else if (data.IsPressed(kFallOver)) {
+ new_superstructure_goal->stilts.unsafe_goal = 0.71;
} else {
new_superstructure_goal->stilts.unsafe_goal = 0.01;
}
- if (data.IsPressed(kIntakeOut)) {
- new_superstructure_goal->intake.unsafe_goal = 0.8;
- new_superstructure_goal->roller_voltage = 5.0;
- } else {
- new_superstructure_goal->intake.unsafe_goal = -1.2;
- new_superstructure_goal->roller_voltage = 0.0;
- }
-
- if (data.IsPressed(kElevator1)) {
+ if (data.IsPressed(kElevatorFront1)) {
elevator_height_ = 1.5;
- } else if (data.IsPressed(kElevator2)) {
+ } else if (data.IsPressed(kElevatorFront2)) {
elevator_height_ = 1.2;
- } else if (data.IsPressed(kElevator3)) {
+ } else if (data.IsPressed(kElevatorFront3)) {
elevator_height_ = 0.8;
- } else if (data.IsPressed(kElevator4)) {
+ } else if (data.IsPressed(kElevatorFront4)) {
elevator_height_ = 0.3;
- } else if (data.IsPressed(kElevator5)) {
+ } else if (data.IsPressed(kElevatorFront5)) {
elevator_height_ = 0.01;
}
+ /*
if (data.IsPressed(kWristDown)) {
wrist_angle_ = -M_PI / 3.0;
} else {
wrist_angle_ = M_PI / 3.0;
}
+ */
+ if (data.IsPressed(kWristBackwards)) {
+ // Hatch pannel back
+ elevator_height_ = 0.03;
+ wrist_angle_ = -M_PI / 2.0;
+ Disc();
+ } else if (data.IsPressed(kWristForwards)) {
+ // Hatch pannel front
+ elevator_height_ = 0.03;
+ wrist_angle_ = M_PI / 2.0;
+ Disc();
+ } else if (data.IsPressed(kElevatorFront5)) {
+ // Ball front
+ Ball();
+ elevator_height_ = 0.52;
+ wrist_angle_ = 1.1;
+ } else if (data.IsPressed(kElevatorBack5)) {
+ // Ball back
+ elevator_height_ = 0.52;
+ wrist_angle_ = -1.1;
+ } else if (data.IsPressed(kElevatorFront2)) {
+ elevator_height_ = 1.5;
+ wrist_angle_ = 0.0;
+ } else {
+ wrist_angle_ = 0.0;
+ elevator_height_ = 0.36;
+ }
+ //if (data.IsPressed(kElevatorIntaking)) {
+ //}
+ if (data.IsPressed(kIntakeOut) && !superstructure_queue.status->has_piece) {
+ elevator_height_ = 0.29;
+ wrist_angle_ = 2.14;
+ new_superstructure_goal->intake.unsafe_goal = 0.52;
+ if (data.IsPressed(kElevatorIntaking)) {
+ new_superstructure_goal->roller_voltage = 6.0;
+ } else {
+ new_superstructure_goal->roller_voltage = 0.0;
+ }
+ Ball();
+ } else {
+ new_superstructure_goal->intake.unsafe_goal = -1.2;
+ new_superstructure_goal->roller_voltage = 0.0;
+ }
+
+ if (data.IsPressed(kElevatorIntakingUp)) {
+ elevator_height_ = 0.29 + 0.3;
+ wrist_angle_ = 2.14;
+ }
+
+
+ if (data.IsPressed(kRelease)) {
+ top_ = false;
+ bottom_ = false;
+ }
+
+ new_superstructure_goal->suction.top = top_;
+ new_superstructure_goal->suction.bottom = bottom_;
new_superstructure_goal->elevator.unsafe_goal = elevator_height_;
new_superstructure_goal->wrist.unsafe_goal = wrist_angle_;
@@ -199,10 +279,22 @@
}
}
+ void Disc() {
+ top_ = true;
+ bottom_ = true;
+ }
+ void Ball() {
+ top_ = false;
+ bottom_ = true;
+ }
+
private:
// Current goals here.
double elevator_height_ = 0.0;
double wrist_angle_ = 0.0;
+
+ bool top_ = false;
+ bool bottom_ = false;
};
} // namespace joysticks
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 339a388..b3969fb 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -194,6 +194,11 @@
stilts_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ // Vacuum pressure sensor
+ void set_vacuum_sensor(int port) {
+ vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
+ }
+
void RunIteration() override {
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -236,6 +241,13 @@
Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
values.stilts.potentiometer_offset);
+ // Suction
+ constexpr float kMinVoltage = 0.5;
+ constexpr float kMaxVoltage = 2.1;
+ superstructure_message->suction_pressure =
+ (vacuum_sensor_->GetVoltage() - kMinVoltage) /
+ (kMaxVoltage - kMinVoltage);
+
superstructure_message.Send();
}
}
@@ -244,6 +256,8 @@
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
wrist_encoder_, stilts_encoder_;
+ ::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
+
::frc971::wpilib::AbsoluteEncoder intake_encoder_;
// TODO(sabina): Add wrist and elevator hall effects.
};
@@ -254,6 +268,10 @@
elevator_victor_ = ::std::move(t);
}
+ void set_suction_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ suction_victor_ = ::std::move(t);
+ }
+
void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
intake_victor_ = ::std::move(t);
}
@@ -267,12 +285,12 @@
}
private:
- virtual void Read() override {
+ void Read() override {
::y2019::control_loops::superstructure::superstructure_queue.output
.FetchAnother();
}
- virtual void Write() override {
+ void Write() override {
auto &queue =
::y2019::control_loops::superstructure::superstructure_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
@@ -292,19 +310,24 @@
stilts_victor_->SetSpeed(::aos::Clip(queue->stilts_voltage,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
+
+ suction_victor_->SetSpeed(
+ ::aos::Clip(queue->pump_voltage, -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
}
- virtual void Stop() override {
+ void Stop() override {
LOG(WARNING, "Superstructure output too old.\n");
elevator_victor_->SetDisabled();
intake_victor_->SetDisabled();
wrist_victor_->SetDisabled();
stilts_victor_->SetDisabled();
+ suction_victor_->SetDisabled();
}
::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
- wrist_victor_, stilts_victor_;
+ wrist_victor_, stilts_victor_, suction_victor_;
};
class SolenoidWriter {
@@ -314,11 +337,13 @@
".y2019.control_loops.superstructure.superstructure_queue.output") {
}
- void set_big_suction_cup(int index) {
- big_suction_cup_ = pcm_.MakeSolenoid(index);
+ void set_big_suction_cup(int index0, int index1) {
+ big_suction_cup0_ = pcm_.MakeSolenoid(index0);
+ big_suction_cup1_ = pcm_.MakeSolenoid(index1);
}
- void set_small_suction_cup(int index) {
- small_suction_cup_ = pcm_.MakeSolenoid(index);
+ void set_small_suction_cup(int index0, int index1) {
+ small_suction_cup0_ = pcm_.MakeSolenoid(index0);
+ small_suction_cup1_ = pcm_.MakeSolenoid(index1);
}
void set_intake_roller_talon(
@@ -348,8 +373,10 @@
if (superstructure_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
- big_suction_cup_->Set(!superstructure_->intake_suction_top);
- small_suction_cup_->Set(!superstructure_->intake_suction_bottom);
+ big_suction_cup0_->Set(!superstructure_->intake_suction_top);
+ big_suction_cup1_->Set(!superstructure_->intake_suction_top);
+ small_suction_cup0_->Set(!superstructure_->intake_suction_bottom);
+ small_suction_cup1_->Set(!superstructure_->intake_suction_bottom);
intake_rollers_talon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
@@ -374,8 +401,8 @@
private:
::frc971::wpilib::BufferedPcm pcm_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> big_suction_cup_,
- small_suction_cup_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> big_suction_cup0_,
+ big_suction_cup1_, small_suction_cup0_, small_suction_cup1_;
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX>
intake_rollers_talon_;
@@ -424,6 +451,7 @@
reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
+ reader.set_vacuum_sensor(7);
::std::thread reader_thread(::std::ref(reader));
@@ -450,8 +478,8 @@
superstructure_writer.set_elevator_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
// TODO(austin): Do the vacuum
- //superstructure_writer.set_vacuum(
- //::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
+ superstructure_writer.set_suction_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
superstructure_writer.set_intake_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
superstructure_writer.set_wrist_victor(
@@ -465,8 +493,8 @@
SolenoidWriter solenoid_writer;
solenoid_writer.set_intake_roller_talon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));
- solenoid_writer.set_big_suction_cup(0);
- solenoid_writer.set_small_suction_cup(1);
+ solenoid_writer.set_big_suction_cup(0, 1);
+ solenoid_writer.set_small_suction_cup(2, 3);
::std::thread solenoid_writer_thread(::std::ref(solenoid_writer));