Third robot bringup adjustments
Change-Id: I48d28520d4b7e068029b5a0fbb34ca93dbdc85d6
diff --git a/y2017_bot3/BUILD b/y2017_bot3/BUILD
index 9fc50f8..5f952ac 100644
--- a/y2017_bot3/BUILD
+++ b/y2017_bot3/BUILD
@@ -1,4 +1,4 @@
-load('/aos/downloader/downloader', 'aos_downloader')
+load('//aos/downloader:downloader.bzl', 'aos_downloader')
cc_binary(
name = 'joystick_reader',
@@ -16,6 +16,7 @@
'//frc971/autonomous:auto_queue',
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/autonomous:base_autonomous_actor',
+ '//y2017_bot3/control_loops/drivetrain:drivetrain_base',
'//y2017_bot3/control_loops/superstructure:superstructure_queue',
'//y2017_bot3/control_loops/superstructure:superstructure_lib',
],
@@ -62,10 +63,11 @@
aos_downloader(
name = 'download',
start_srcs = [
+ ':wpilib_interface',
+ ':joystick_reader',
'//aos:prime_start_binaries',
'//y2017_bot3/control_loops/drivetrain:drivetrain',
'//y2017_bot3/control_loops/superstructure:superstructure',
- ':wpilib_interface',
],
srcs = [
'//aos:prime_binaries',
@@ -76,10 +78,11 @@
aos_downloader(
name = 'download_stripped',
start_srcs = [
+ ':wpilib_interface.stripped',
+ ':joystick_reader.stripped',
'//aos:prime_start_binaries_stripped',
'//y2017_bot3/control_loops/drivetrain:drivetrain.stripped',
'//y2017_bot3/control_loops/superstructure:superstructure.stripped',
- ':wpilib_interface.stripped',
],
srcs = [
'//aos:prime_binaries_stripped',
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
index 9c3e66e..1926fe6 100644
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -19,7 +19,7 @@
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
- ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
+ ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.cc b/y2017_bot3/control_loops/superstructure/superstructure.cc
index 9cb069b..65dc0da 100644
--- a/y2017_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2017_bot3/control_loops/superstructure/superstructure.cc
@@ -26,9 +26,9 @@
if (unsafe_goal) {
// Intake.
output->voltage_rollers = unsafe_goal->voltage_rollers;
-
+ // Fire piston to release gear.
output->fingers_out = unsafe_goal->fingers_out;
-
+ // Spin Hanger.
output->hanger_voltage = unsafe_goal->hanger_voltage;
}
}
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.q b/y2017_bot3/control_loops/superstructure/superstructure.q
index 3de8f68..05249d2 100644
--- a/y2017_bot3/control_loops/superstructure/superstructure.q
+++ b/y2017_bot3/control_loops/superstructure/superstructure.q
@@ -37,4 +37,3 @@
};
queue_group SuperstructureQueue superstructure_queue;
-
diff --git a/y2017_bot3/joystick_reader.cc b/y2017_bot3/joystick_reader.cc
index 40a8d12..ff3961c 100644
--- a/y2017_bot3/joystick_reader.cc
+++ b/y2017_bot3/joystick_reader.cc
@@ -15,6 +15,7 @@
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
+#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
using ::frc971::control_loops::drivetrain_queue;
using ::y2017_bot3::control_loops::superstructure_queue;
@@ -29,19 +30,20 @@
namespace input {
namespace joysticks {
-const ButtonLocation kHangerOn(3, 1);
-const ButtonLocation kRollersOn(3, 2);
-const ButtonLocation kGearOut(3, 3);
-
-const ButtonLocation kRollerOn(3, 4);
+const ButtonLocation kHangerOn(2, 11);
+const ButtonLocation kGearOut(2, 10);
+const ButtonLocation kRollerOn(2, 7);
+const ButtonLocation kRollerSpit(2, 6);
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
class Reader : public ::aos::input::JoystickInput {
public:
Reader() {
+ // Setting driver station type to Steering Wheel
drivetrain_input_reader_ = DrivetrainInputReader::Make(
- DrivetrainInputReader::InputType::kSteeringWheel);
+ DrivetrainInputReader::InputType::kSteeringWheel,
+ ::y2017_bot3::control_loops::drivetrain::GetDrivetrainConfig());
}
void RunIteration(const ::aos::input::driver_station::Data &data) override {
@@ -69,8 +71,12 @@
// Process pending actions.
action_queue_.Tick();
was_running_ = action_queue_.Running();
- }
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+ }
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
drivetrain_input_reader_->HandleDrivetrain(data);
robot_velocity_ = drivetrain_input_reader_->robot_velocity();
@@ -90,6 +96,10 @@
new_superstructure_goal->voltage_rollers = 12.0;
}
+ if (data.IsPressed(kRollerSpit)) {
+ new_superstructure_goal->voltage_rollers = -12.0;
+ }
+
if (data.IsPressed(kHangerOn)) {
new_superstructure_goal->hanger_voltage = 12.0;
}
diff --git a/y2017_bot3/wpilib_interface.cc b/y2017_bot3/wpilib_interface.cc
index 7fa160c..8fd2488 100644
--- a/y2017_bot3/wpilib_interface.cc
+++ b/y2017_bot3/wpilib_interface.cc
@@ -65,11 +65,10 @@
constexpr double kMaxBringupPower = 12.0;
-constexpr double kDrivetrainCyclesPerRevolution = 256;
+constexpr double kDrivetrainCyclesPerRevolution = 128.0;
constexpr double kDrivetrainEncoderCountsPerRevolution =
kDrivetrainCyclesPerRevolution * 4;
-constexpr double kDrivetrainEncoderRatio =
- 1.0 * control_loops::drivetrain::kWheelRadius;
+constexpr double kDrivetrainEncoderRatio = 1.0;
constexpr double kMaxDrivetrainEncoderPulsesPerSecond =
control_loops::drivetrain::kFreeSpeed *
control_loops::drivetrain::kHighOutputRatio /
@@ -104,7 +103,7 @@
}
double drivetrain_translate(int32_t in) {
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
+ return -static_cast<double>(in) / (kDrivetrainCyclesPerRevolution /*cpr*/ * 4.0 /*4x*/) *
kDrivetrainEncoderRatio *
control_loops::drivetrain::kWheelRadius *
2.0 * M_PI;
@@ -288,12 +287,12 @@
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
drivetrain_message->right_encoder =
- drivetrain_translate(drivetrain_right_encoder_->GetRaw());
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
drivetrain_message->right_speed =
drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
drivetrain_message->left_encoder =
- -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
+ drivetrain_translate(drivetrain_left_encoder_->GetRaw());
drivetrain_message->left_speed =
drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
@@ -349,7 +348,7 @@
public:
SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
- drivetrain_(".y2017_bot3.control_loops.drivetrain_queue.output"),
+ drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
superstructure_(
".y2017_bot3.control_loops.superstructure_queue.output") {}
@@ -357,9 +356,14 @@
compressor_ = ::std::move(compressor);
}
- void set_drivetrain_shifter(
+ void set_left_drivetrain_shifter(
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- drivetrain_shifter_ = ::std::move(s);
+ left_drivetrain_shifter_ = ::std::move(s);
+ }
+
+ void set_right_drivetrain_shifter(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ right_drivetrain_shifter_ = ::std::move(s);
}
void set_fingers(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
@@ -386,8 +390,8 @@
drivetrain_.FetchLatest();
if (drivetrain_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- drivetrain_shifter_->Set(
- !(drivetrain_->left_high || drivetrain_->right_high));
+ left_drivetrain_shifter_->Set(!drivetrain_->left_high);
+ right_drivetrain_shifter_->Set(!drivetrain_->right_high);
}
}
@@ -414,7 +418,10 @@
private:
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_shifter_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid>
+ left_drivetrain_shifter_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid>
+ right_drivetrain_shifter_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> fingers_;
::std::unique_ptr<Compressor> compressor_;
@@ -428,13 +435,19 @@
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
- void set_drivetrain_left_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_left_victor_ = ::std::move(t);
+ void set_drivetrain_left0_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ drivetrain_left0_victor_ = ::std::move(t);
+ }
+ void set_drivetrain_left1_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ drivetrain_left1_victor_ = ::std::move(t);
}
- void set_drivetrain_right_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_right_victor_ = ::std::move(t);
- };
+ void set_drivetrain_right0_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ drivetrain_right0_victor_ = ::std::move(t);
+ }
+ void set_drivetrain_right1_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ drivetrain_right1_victor_ = ::std::move(t);
+ }
private:
virtual void Read() override {
@@ -444,18 +457,23 @@
virtual void Write() override {
auto &queue = ::frc971::control_loops::drivetrain_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_victor_->SetSpeed(-queue->left_voltage / 12.0);
- drivetrain_right_victor_->SetSpeed(queue->right_voltage / 12.0);
+ drivetrain_left0_victor_->SetSpeed(queue->left_voltage / 12.0);
+ drivetrain_left1_victor_->SetSpeed(queue->left_voltage / 12.0);
+ drivetrain_right0_victor_->SetSpeed(-queue->right_voltage / 12.0);
+ drivetrain_right1_victor_->SetSpeed(-queue->right_voltage / 12.0);
}
virtual void Stop() override {
LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left_victor_->SetDisabled();
- drivetrain_right_victor_->SetDisabled();
+ drivetrain_left0_victor_->SetDisabled();
+ drivetrain_left1_victor_->SetDisabled();
+ drivetrain_right0_victor_->SetDisabled();
+ drivetrain_right1_victor_->SetDisabled();
}
- ::std::unique_ptr<::frc::VictorSP> drivetrain_left_victor_,
- drivetrain_right_victor_;
+ ::std::unique_ptr<::frc::VictorSP> drivetrain_left0_victor_,
+ drivetrain_left1_victor_, drivetrain_right0_victor_,
+ drivetrain_right1_victor_;
};
class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
@@ -464,9 +482,13 @@
rollers_victor_ = ::std::move(t);
}
- void set_hanger_victor(::std::unique_ptr<::frc::VictorSP> t) {
- hanger_victor_ = ::std::move(t);
- }
+ void set_hanger0_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ hanger0_victor_ = ::std::move(t);
+ }
+ void set_hanger1_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ hanger1_victor_ = ::std::move(t);
+ }
+
private:
virtual void Read() override {
::y2017_bot3::control_loops::superstructure_queue.output.FetchAnother();
@@ -476,17 +498,19 @@
auto &queue = ::y2017_bot3::control_loops::superstructure_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
rollers_victor_->SetSpeed(queue->voltage_rollers / 12.0);
- hanger_victor_->SetSpeed(queue->hanger_voltage / 12.0);
+ hanger0_victor_->SetSpeed(queue->hanger_voltage / 12.0);
+ hanger1_victor_->SetSpeed(queue->hanger_voltage / 12.0);
}
virtual void Stop() override {
LOG(WARNING, "Superstructure output too old.\n");
rollers_victor_->SetDisabled();
- hanger_victor_->SetDisabled();
+ hanger0_victor_->SetDisabled();
+ hanger1_victor_->SetDisabled();
}
::std::unique_ptr<::frc::VictorSP> rollers_victor_;
- ::std::unique_ptr<::frc::VictorSP> hanger_victor_;
+ ::std::unique_ptr<::frc::VictorSP> hanger0_victor_, hanger1_victor_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -507,7 +531,6 @@
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
SensorReader reader;
- // TODO(sabina): Update port numbers
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
reader.set_drivetrain_left_hall(make_unique<AnalogInput>(0));
@@ -521,22 +544,30 @@
::std::thread gyro_thread(::std::ref(gyro_sender));
DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_drivetrain_left_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)));
- drivetrain_writer.set_drivetrain_right_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)));
+ drivetrain_writer.set_drivetrain_left0_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(5)));
+ drivetrain_writer.set_drivetrain_left1_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
+ drivetrain_writer.set_drivetrain_right0_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
+ drivetrain_writer.set_drivetrain_right1_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
SuperstructureWriter superstructure_writer;
superstructure_writer.set_rollers_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
- superstructure_writer.set_hanger_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ superstructure_writer.set_hanger0_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)));
+ superstructure_writer.set_hanger1_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)));
+ ::std::thread superstructure_writer_thread(::std::ref(superstructure_writer));
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(pcm);
- solenoid_writer.set_drivetrain_shifter(pcm->MakeSolenoid(0));
+ solenoid_writer.set_left_drivetrain_shifter(pcm->MakeSolenoid(3));
+ solenoid_writer.set_right_drivetrain_shifter(pcm->MakeSolenoid(1));
solenoid_writer.set_fingers(pcm->MakeSolenoid(2));
solenoid_writer.set_compressor(make_unique<Compressor>());