Third robot bringup adjustments
Change-Id: I48d28520d4b7e068029b5a0fbb34ca93dbdc85d6
diff --git a/aos/input/BUILD b/aos/input/BUILD
index d8fc2c8..ad1915b 100644
--- a/aos/input/BUILD
+++ b/aos/input/BUILD
@@ -33,5 +33,6 @@
'//aos/common/network:socket',
'//aos/common:math',
'//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_config',
],
)
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index a0d95cf..1f6d713 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -19,6 +19,8 @@
namespace aos {
namespace input {
+const ButtonLocation kShiftHigh(2, 3), kShiftHigh2(2, 2), kShiftLow(2, 1);
+
void DrivetrainInputReader::HandleDrivetrain(
const ::aos::input::driver_station::Data &data) {
bool is_control_loop_driving = false;
@@ -26,13 +28,14 @@
const auto wheel_and_throttle = GetWheelAndThrottle(data);
const double wheel = wheel_and_throttle.wheel;
const double throttle = wheel_and_throttle.throttle;
+ const bool high_gear = wheel_and_throttle.high_gear;
drivetrain_queue.status.FetchLatest();
if (drivetrain_queue.status.get()) {
robot_velocity_ = drivetrain_queue.status->robot_speed;
}
- if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
+ if (data.PosEdge(turn1_) || data.PosEdge(turn2_)) {
if (drivetrain_queue.status.get()) {
left_goal_ = drivetrain_queue.status->estimated_left_position;
right_goal_ = drivetrain_queue.status->estimated_right_position;
@@ -42,13 +45,14 @@
left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
const double current_right_goal =
right_goal_ + wheel * wheel_multiplier_ + throttle * 0.3;
- if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
+ if (data.IsPressed(turn1_) || data.IsPressed(turn2_)) {
is_control_loop_driving = true;
}
auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
new_drivetrain_goal->steering = wheel;
new_drivetrain_goal->throttle = throttle;
- new_drivetrain_goal->quickturn = data.IsPressed(kQuickTurn);
+ new_drivetrain_goal->highgear = high_gear;
+ new_drivetrain_goal->quickturn = data.IsPressed(quick_turn_);
new_drivetrain_goal->control_loop_driving = is_control_loop_driving;
new_drivetrain_goal->left_goal = current_left_goal;
new_drivetrain_goal->right_goal = current_right_goal;
@@ -66,15 +70,28 @@
DrivetrainInputReader::WheelAndThrottle
SteeringWheelDrivetrainInputReader::GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) {
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
- return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+ const double wheel = -data.GetAxis(steering_wheel_);
+ const double throttle = -data.GetAxis(drive_throttle_);
+
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ high_gear_ = default_high_gear_;
+ }
+
+ if (data.PosEdge(kShiftLow)) {
+ high_gear_ = false;
+ }
+
+ if (data.PosEdge(kShiftHigh) || data.PosEdge(kShiftHigh2)) {
+ high_gear_ = true;
+ }
+
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle, high_gear_};
}
DrivetrainInputReader::WheelAndThrottle
PistolDrivetrainInputReader::GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) {
- const double unscaled_wheel = data.GetAxis(kSteeringWheel);
+ const double unscaled_wheel = data.GetAxis(steering_wheel_);
double wheel;
if (unscaled_wheel < 0.0) {
wheel = unscaled_wheel / 0.484375;
@@ -82,7 +99,7 @@
wheel = unscaled_wheel / 0.385827;
}
- const double unscaled_throttle = -data.GetAxis(kDriveThrottle);
+ const double unscaled_throttle = -data.GetAxis(drive_throttle_);
double unmodified_throttle;
if (unscaled_throttle < 0.0) {
unmodified_throttle = unscaled_throttle / 0.086614;
@@ -98,7 +115,7 @@
::std::sin(throttle_range);
throttle = ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
throttle = 2.0 * unmodified_throttle - throttle;
- return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle, true};
}
DrivetrainInputReader::WheelAndThrottle
@@ -108,10 +125,10 @@
constexpr double kWheelDeadband = 0.05;
constexpr double kThrottleDeadband = 0.05;
const double wheel =
- aos::Deadband(-data.GetAxis(kSteeringWheel), kWheelDeadband, 1.0);
+ aos::Deadband(-data.GetAxis(steering_wheel_), kWheelDeadband, 1.0);
const double unmodified_throttle =
- aos::Deadband(-data.GetAxis(kDriveThrottle), kThrottleDeadband, 1.0);
+ aos::Deadband(-data.GetAxis(drive_throttle_), kThrottleDeadband, 1.0);
// Apply a sin function that's scaled to make it feel better.
constexpr double throttle_range = M_PI_2 * 0.9;
@@ -120,11 +137,11 @@
::std::sin(throttle_range);
throttle = ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
throttle = 2.0 * unmodified_throttle - throttle;
- return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle, true};
}
std::unique_ptr<SteeringWheelDrivetrainInputReader>
-SteeringWheelDrivetrainInputReader::Make() {
+SteeringWheelDrivetrainInputReader::Make(bool default_high_gear) {
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kQuickTurn(1, 5);
const ButtonLocation kTurn1(1, 7);
@@ -132,6 +149,8 @@
std::unique_ptr<SteeringWheelDrivetrainInputReader> result(
new SteeringWheelDrivetrainInputReader(kSteeringWheel, kDriveThrottle,
kQuickTurn, kTurn1, kTurn2));
+ result.get()->set_default_high_gear(default_high_gear);
+
return result;
}
@@ -166,13 +185,16 @@
return result;
}
::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
- InputType type) {
+ InputType type,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig &dt_config) {
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader;
using InputType = DrivetrainInputReader::InputType;
+
switch (type) {
case InputType::kSteeringWheel:
- drivetrain_input_reader = SteeringWheelDrivetrainInputReader::Make();
+ drivetrain_input_reader =
+ SteeringWheelDrivetrainInputReader::Make(dt_config.default_high_gear);
break;
case InputType::kPistol:
drivetrain_input_reader = PistolDrivetrainInputReader::Make();
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 4b13ac6..062332d 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -10,6 +10,7 @@
#include "aos/common/input/driver_station_data.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
namespace aos {
namespace input {
@@ -35,16 +36,16 @@
class DrivetrainInputReader {
public:
// Inputs driver station button and joystick locations
- DrivetrainInputReader(driver_station::JoystickAxis kSteeringWheel,
- driver_station::JoystickAxis kDriveThrottle,
- driver_station::ButtonLocation kQuickTurn,
- driver_station::ButtonLocation kTurn1,
- driver_station::ButtonLocation kTurn2)
- : kSteeringWheel(kSteeringWheel),
- kDriveThrottle(kDriveThrottle),
- kQuickTurn(kQuickTurn),
- kTurn1(kTurn1),
- kTurn2(kTurn2) {}
+ DrivetrainInputReader(driver_station::JoystickAxis steering_wheel,
+ driver_station::JoystickAxis drive_throttle,
+ driver_station::ButtonLocation quick_turn,
+ driver_station::ButtonLocation turn1,
+ driver_station::ButtonLocation turn2)
+ : steering_wheel_(steering_wheel),
+ drive_throttle_(drive_throttle),
+ quick_turn_(quick_turn),
+ turn1_(turn1),
+ turn2_(turn2) {}
virtual ~DrivetrainInputReader() = default;
@@ -56,7 +57,9 @@
};
// Constructs the appropriate DrivetrainInputReader.
- static std::unique_ptr<DrivetrainInputReader> Make(InputType type);
+ static std::unique_ptr<DrivetrainInputReader> Make(
+ InputType type,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig &dt_config);
// Processes new joystick data and publishes drivetrain goal messages.
void HandleDrivetrain(const ::aos::input::driver_station::Data &data);
@@ -71,17 +74,18 @@
double robot_velocity() const { return robot_velocity_; }
protected:
- const driver_station::JoystickAxis kSteeringWheel;
- const driver_station::JoystickAxis kDriveThrottle;
- const driver_station::ButtonLocation kQuickTurn;
- const driver_station::ButtonLocation kTurn1;
- const driver_station::ButtonLocation kTurn2;
+ const driver_station::JoystickAxis steering_wheel_;
+ const driver_station::JoystickAxis drive_throttle_;
+ const driver_station::ButtonLocation quick_turn_;
+ const driver_station::ButtonLocation turn1_;
+ const driver_station::ButtonLocation turn2_;
// Structure containing the (potentially adjusted) steering and throttle
// values from the joysticks.
struct WheelAndThrottle {
double wheel;
double throttle;
+ bool high_gear;
};
private:
@@ -105,11 +109,20 @@
// Creates a DrivetrainInputReader with the corresponding joystick ports and
// axis for the big steering wheel and throttle stick.
- static std::unique_ptr<SteeringWheelDrivetrainInputReader> Make();
+ static std::unique_ptr<SteeringWheelDrivetrainInputReader> Make(
+ bool default_high_gear);
+
+ // Sets the default shifter position
+ void set_default_high_gear(bool default_high_gear) {
+ high_gear_ = default_high_gear;
+ default_high_gear_ = default_high_gear;
+ }
private:
WheelAndThrottle GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) override;
+ bool high_gear_;
+ bool default_high_gear_;
};
class PistolDrivetrainInputReader : public DrivetrainInputReader {
diff --git a/y2017/BUILD b/y2017/BUILD
index 763e331..d202064 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -42,6 +42,7 @@
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//y2017/actors:autonomous_action_lib',
'//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017/control_loops/drivetrain:drivetrain_base',
],
)
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index e0f66ba..b47c733 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -1,7 +1,7 @@
+#include <math.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
-#include <math.h>
#include "aos/common/actions/actions.h"
#include "aos/common/input/driver_station_data.h"
@@ -16,6 +16,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/drivetrain/drivetrain_base.h"
using ::frc971::control_loops::drivetrain_queue;
using ::y2017::control_loops::superstructure_queue;
@@ -54,7 +55,8 @@
public:
Reader() {
drivetrain_input_reader_ = DrivetrainInputReader::Make(
- DrivetrainInputReader::InputType::kSteeringWheel);
+ DrivetrainInputReader::InputType::kSteeringWheel,
+ ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
}
enum class ShotDistance { VISION_SHOT, MIDDLE_SHOT, FAR_SHOT };
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>());