Merge "Fix up drivetrain constants"
diff --git a/y2016/constants.h b/y2016/constants.h
index ba16902..ed86d0c 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -37,34 +37,40 @@
// The ratio from the encoder shaft to the drivetrain wheels.
static constexpr double kDrivetrainEncoderRatio =
- (18.0 / 50.0) /*output reduction*/ * (56.0 / 30.0);
+ (18.0 / 36.0) /*output reduction*/ * (44.0 / 30.0);
// The gear ratios from motor shafts to the drivetrain wheels for high and low
// gear.
- static constexpr double kLowGearRatio = 18.0 / 60.0 * 18.0 / 50.0;
- static constexpr double kHighGearRatio = 28.0 / 50.0 * 18.0 / 50.0;
+ static constexpr double kLowGearRatio = 18.0 / 60.0 * 18.0 / 36.0;
+ static constexpr double kHighGearRatio = 28.0 / 50.0 * 18.0 / 36.0;
- static constexpr double kTurnWidth = 25.0 / 100.0 * 2.54; // Robot width.
+ static constexpr double kTurnWidth = 0.601; // Robot width.
// Ratios for our subsystems.
static constexpr double kShooterEncoderRatio = 1.0;
- static constexpr double kIntakeEncoderRatio = 16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0;
- static constexpr double kShoulderEncoderRatio = 16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0;
- static constexpr double kWristEncoderRatio = 16.0 / 48.0 * 18.0 / 64.0 * 14.0 / 54.0;
+ static constexpr double kIntakeEncoderRatio =
+ 16.0 / 48.0 * 18.0 / 72.0 * 14.0 / 64.0;
+ static constexpr double kShoulderEncoderRatio =
+ 16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0;
+ static constexpr double kWristEncoderRatio =
+ 16.0 / 48.0 * 18.0 / 64.0 * 14.0 / 54.0;
- static constexpr double kIntakePotRatio = 16.0 / 58.0;
+ static constexpr double kIntakePotRatio = 16.0 / 48.0;
static constexpr double kShoulderPotRatio = 16.0 / 58.0;
static constexpr double kWristPotRatio = 16.0 / 48.0;
// Difference in radians between index pulses.
- static constexpr double kIntakeEncoderIndexDifference = 2.0 * M_PI * kIntakeEncoderRatio;
- static constexpr double kShoulderEncoderIndexDifference = 2.0 * M_PI * kShoulderEncoderRatio;
- static constexpr double kWristEncoderIndexDifference = 2.0 * M_PI * kWristEncoderRatio;
+ static constexpr double kIntakeEncoderIndexDifference =
+ 2.0 * M_PI * kIntakeEncoderRatio;
+ static constexpr double kShoulderEncoderIndexDifference =
+ 2.0 * M_PI * kShoulderEncoderRatio;
+ static constexpr double kWristEncoderIndexDifference =
+ 2.0 * M_PI * kWristEncoderRatio;
// Subsystem motion ranges, in whatever units that their respective queues say
// the use.
- static constexpr Range kIntakeRange{-0.4, 2.0, -0.3, 1.9};
- static constexpr Range kShoulderRange{-0.2, 2.0, -0.1, 1.9};
+ static constexpr Range kIntakeRange{-0.270, 2.0, -0.200, 1.9};
+ static constexpr Range kShoulderRange{-0.050, 2.0, 0.000, 1.9};
static constexpr Range kWristRange{-2.0, 2.0, -1.9, 1.0};
// ///// Dynamic constants. /////
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 6550fc2..0e8f3a5 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -92,6 +92,11 @@
if (output) {
output->voltage_left = left_.voltage();
output->voltage_right = right_.voltage();
+
+ if (goal) {
+ output->clamp_open = goal->clamp_open;
+ output->push_to_shooter = goal->push_to_shooter;
+ }
}
}
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
index af3f064..e0b4531 100644
--- a/y2016/control_loops/shooter/shooter.q
+++ b/y2016/control_loops/shooter/shooter.q
@@ -21,6 +21,9 @@
message Goal {
// Angular velocity goals in radians/second.
double angular_velocity;
+
+ bool clamp_open; // True to release our clamp on the ball.
+ bool push_to_shooter; // True to push the ball into the shooter.
};
message Position {
@@ -44,6 +47,10 @@
// Voltage in volts of the left and right shooter motors.
double voltage_left;
double voltage_right;
+
+ // See comments on the identical fields in Goal for details.
+ bool clamp_open;
+ bool push_to_shooter;
};
queue Goal goal;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index fa227bd..1bd4eca 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -81,8 +81,7 @@
SuperstructureSimulation()
: intake_plant_(new IntakePlant(MakeIntakePlant())),
plant_arm_(new ArmPlant(MakeArmPlant())),
- pot_encoder_intake_(
- constants::Values::kIntakeEncoderIndexDifference),
+ pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
pot_encoder_shoulder_(
constants::Values::kShoulderEncoderIndexDifference),
pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference),
@@ -245,9 +244,9 @@
// Tests that the superstructure does nothing when the goal is zero.
TEST_F(SuperstructureTest, DoesNothing) {
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
.max_angular_velocity_intake(20)
.max_angular_velocity_shoulder(20)
.max_angular_velocity_wrist(20)
@@ -306,7 +305,8 @@
superstructure_queue_.status->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(constants::Values::kWristRange.upper + constants::Values::kShoulderRange.upper,
+ EXPECT_NEAR(constants::Values::kWristRange.upper +
+ constants::Values::kShoulderRange.upper,
superstructure_queue_.status->wrist.angle, 0.001);
// Set some ridiculous goals to test lower limits.
@@ -330,16 +330,17 @@
superstructure_queue_.status->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.lower,
superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(constants::Values::kWristRange.lower + constants::Values::kShoulderRange.lower,
+ EXPECT_NEAR(constants::Values::kWristRange.lower +
+ constants::Values::kShoulderRange.lower,
superstructure_queue_.status->wrist.angle, 0.001);
}
// Tests that the loop zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
.max_angular_velocity_shoulder(20)
@@ -369,9 +370,9 @@
superstructure_plant_.InitializeWristPosition(
constants::Values::kWristRange.lower);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
RunForTime(Time::InSeconds(5));
@@ -388,9 +389,9 @@
superstructure_plant_.InitializeWristPosition(
constants::Values::kWristRange.upper);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
RunForTime(Time::InSeconds(5));
@@ -406,11 +407,12 @@
constants::Values::kShoulderRange.upper);
superstructure_plant_.InitializeWristPosition(
constants::Values::kWristRange.upper);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.3)
- .angle_shoulder(0.3)
- .angle_wrist(0.3)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.lower + 0.3)
+ .angle_shoulder(constants::Values::kShoulderRange.lower + 0.3)
+ .angle_wrist(constants::Values::kWristRange.lower + 0.3)
+ .Send());
RunForTime(Time::InSeconds(5));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
@@ -425,11 +427,12 @@
// Tests that the internal goals don't change while disabled.
TEST_F(SuperstructureTest, DisabledGoalTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.03)
- .angle_shoulder(0.03)
- .angle_wrist(0.03)
- .Send());
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.lower + 0.03)
+ .angle_shoulder(constants::Values::kShoulderRange.lower + 0.03)
+ .angle_wrist(constants::Values::kWristRange.lower + 0.03)
+ .Send());
RunForTime(Time::InMS(100), false);
EXPECT_EQ(0.0, superstructure_.intake_.goal(0, 0));
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 156f90a..8ec212f 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -32,6 +32,7 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/constants.h"
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
@@ -346,7 +347,8 @@
::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;
- ::frc971::wpilib::DMAEncoderAndPotentiometer intake_encoder_, shoulder_encoder_, wrist_encoder_;
+ ::frc971::wpilib::DMAEncoderAndPotentiometer intake_encoder_,
+ shoulder_encoder_, wrist_encoder_;
::std::atomic<bool> run_{true};
DigitalGlitchFilter encoder_filter_, hall_filter_;
@@ -356,7 +358,8 @@
public:
SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
- drivetrain_(".frc971.control_loops.drivetrain_queue.output") {}
+ drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
+ shooter_(".y2016.control_loops.shooter_queue.output") {}
void set_pressure_switch(::std::unique_ptr<DigitalInput> pressure_switch) {
pressure_switch_ = ::std::move(pressure_switch);
@@ -376,6 +379,16 @@
drivetrain_right_ = ::std::move(s);
}
+ void set_shooter_clamp(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ shooter_clamp_ = ::std::move(s);
+ }
+
+ void set_shooter_pusher(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ shooter_pusher_ = ::std::move(s);
+ }
+
void operator()() {
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(27);
@@ -401,6 +414,15 @@
}
{
+ shooter_.FetchLatest();
+ if (shooter_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *shooter_);
+ shooter_clamp_->Set(shooter_->clamp_open);
+ shooter_pusher_->Set(shooter_->push_to_shooter);
+ }
+ }
+
+ {
::frc971::wpilib::PneumaticsToLog to_log;
{
const bool compressor_on = !pressure_switch_->Get();
@@ -425,12 +447,12 @@
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_left_,
- drivetrain_right_;
-
+ drivetrain_right_, shooter_clamp_, shooter_pusher_;
::std::unique_ptr<DigitalInput> pressure_switch_;
::std::unique_ptr<Relay> compressor_relay_;
::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
+ ::aos::Queue<::y2016::control_loops::ShooterQueue::Output> shooter_;
::std::atomic<bool> run_{true};
};
@@ -608,6 +630,8 @@
SolenoidWriter solenoid_writer(pcm);
solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(6));
solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(7));
+ solenoid_writer.set_shooter_clamp(pcm->MakeSolenoid(6));
+ solenoid_writer.set_shooter_pusher(pcm->MakeSolenoid(7));
solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(25));
solenoid_writer.set_compressor_relay(make_unique<Relay>(0));