Added superstructure
Change-Id: I3fc6b2b555467fad231902fbc50af729b4b7157f
diff --git a/y2017_bot3/BUILD b/y2017_bot3/BUILD
index c1dab0f..9ff13fd 100644
--- a/y2017_bot3/BUILD
+++ b/y2017_bot3/BUILD
@@ -33,6 +33,7 @@
'//frc971/wpilib:wpilib_robot_base',
'//third_party:wpilib',
'//y2017_bot3/control_loops/drivetrain:polydrivetrain_plants',
+ '//y2017_bot3/control_loops/superstructure:superstructure_queue',
],
restricted_to = ['//tools:roborio'],
)
diff --git a/y2017_bot3/control_loops/superstructure/BUILD b/y2017_bot3/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..82b5d7a
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/BUILD
@@ -0,0 +1,47 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'superstructure_queue',
+ srcs = [
+ 'superstructure.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ '//frc971/control_loops:profiled_subsystem_queue',
+ '//frc971/control_loops:queues',
+ ],
+)
+
+cc_library(
+ name = 'superstructure_lib',
+ srcs = [
+ 'superstructure.cc',
+ ],
+ hdrs = [
+ 'superstructure.h',
+ ],
+ deps = [
+ ':superstructure_queue',
+ '//aos/common/controls:control_loop',
+ '//aos/common/util:trapezoid_profile',
+ '//aos/common:math',
+ '//frc971/control_loops:profiled_subsystem',
+ '//frc971/control_loops:simple_capped_state_feedback_loop',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/zeroing',
+ ],
+)
+
+cc_binary(
+ name = 'superstructure',
+ srcs = [
+ 'superstructure_main.cc',
+ ],
+ deps = [
+ '//aos/linux_code:init',
+ ':superstructure_lib',
+ ':superstructure_queue',
+ ],
+)
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.cc b/y2017_bot3/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..9cb069b
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,39 @@
+#include "y2017_bot3/control_loops/superstructure/superstructure.h"
+
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+namespace y2017_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+Superstructure::Superstructure(
+ control_loops::SuperstructureQueue *superstructure_queue)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
+ superstructure_queue) {}
+
+void Superstructure::RunIteration(
+ const control_loops::SuperstructureQueue::Goal *unsafe_goal,
+ const control_loops::SuperstructureQueue::Position * /*position*/,
+ control_loops::SuperstructureQueue::Output *output,
+ control_loops::SuperstructureQueue::Status * /*status*/) {
+ // Write out all the voltages.
+ if (output) {
+ output->voltage_rollers = 0.0;
+ output->hanger_voltage = 0.0;
+ output->fingers_out = false;
+ if (unsafe_goal) {
+ // Intake.
+ output->voltage_rollers = unsafe_goal->voltage_rollers;
+
+ output->fingers_out = unsafe_goal->fingers_out;
+
+ output->hanger_voltage = unsafe_goal->hanger_voltage;
+ }
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017_bot3
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.h b/y2017_bot3/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..79a901b
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/superstructure.h
@@ -0,0 +1,40 @@
+#ifndef Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/zeroing.h"
+#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ public:
+ explicit Superstructure(
+ control_loops::SuperstructureQueue *my_superstructure =
+ &control_loops::superstructure_queue);
+
+ static constexpr double kOperatingVoltage = 12.0;
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::SuperstructureQueue::Goal *unsafe_goal,
+ const control_loops::SuperstructureQueue::Position * /*position*/,
+ control_loops::SuperstructureQueue::Output *output,
+ control_loops::SuperstructureQueue::Status * /*status*/) override;
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017_bot3
+
+#endif // Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.q b/y2017_bot3/control_loops/superstructure/superstructure.q
new file mode 100644
index 0000000..91363c4
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/superstructure.q
@@ -0,0 +1,35 @@
+package y2017_bot3.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group SuperstructureQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // Voltage to send to the rollers. Positive is sucking in.
+ float voltage_rollers;
+ bool fingers_out;
+ float hanger_voltage;
+ };
+
+ message Status {
+ };
+
+ message Position {
+ };
+
+ message Output {
+ float voltage_rollers;
+ bool fingers_out;
+ float hanger_voltage;
+ };
+
+ queue Goal goal;
+ queue Output output;
+ queue Status status;
+ queue Position position;
+};
+
+queue_group SuperstructureQueue superstructure_queue;
+
diff --git a/y2017_bot3/control_loops/superstructure/superstructure_main.cc b/y2017_bot3/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..04a41a4
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,11 @@
+#include "y2017_bot3/control_loops/superstructure/superstructure.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::y2017_bot3::control_loops::superstructure::Superstructure superstructure;
+ superstructure.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2017_bot3/wpilib_interface.cc b/y2017_bot3/wpilib_interface.cc
index 737050a..7fa160c 100644
--- a/y2017_bot3/wpilib_interface.cc
+++ b/y2017_bot3/wpilib_interface.cc
@@ -46,14 +46,16 @@
#include "frc971/wpilib/wpilib_interface.h"
#include "frc971/wpilib/wpilib_robot_base.h"
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2017_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
using ::frc971::control_loops::drivetrain_queue;
+using ::y2017_bot3::control_loops::superstructure_queue;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -102,9 +104,10 @@
}
double drivetrain_translate(int32_t in) {
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
+ return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
kDrivetrainEncoderRatio *
- control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
+ control_loops::drivetrain::kWheelRadius *
+ 2.0 * M_PI;
}
double drivetrain_velocity_translate(double in) {
@@ -302,8 +305,12 @@
drivetrain_message.Send();
}
- dma_synchronizer_->RunIteration();
+ {
+ auto superstructure_message = superstructure_queue.position.MakeMessage();
+ superstructure_message.Send();
}
+ dma_synchronizer_->RunIteration();
+ }
void Quit() { run_ = false; }
@@ -334,16 +341,20 @@
::std::atomic<bool> run_{true};
- DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_, hall_filter_;
+ DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_,
+ hall_filter_;
};
class SolenoidWriter {
public:
SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
- drivetrain_(".y2017_bot3.control_loops.drivetrain_queue.output"){}
+ drivetrain_(".y2017_bot3.control_loops.drivetrain_queue.output"),
+ superstructure_(
+ ".y2017_bot3.control_loops.superstructure_queue.output") {}
+
void set_compressor(::std::unique_ptr<Compressor> compressor) {
- compressor_ = ::std::move(compressor);
+ compressor_ = ::std::move(compressor);
}
void set_drivetrain_shifter(
@@ -351,6 +362,10 @@
drivetrain_shifter_ = ::std::move(s);
}
+ void set_fingers(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ fingers_ = ::std::move(s);
+ }
+
void operator()() {
compressor_->Start();
::aos::SetCurrentThreadName("Solenoids");
@@ -377,10 +392,16 @@
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
- {
- to_log.compressor_on = compressor_->Enabled();
+ superstructure_.FetchLatest();
+ if (superstructure_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+ fingers_->Set(superstructure_->fingers_out);
}
+ }
+
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+ { to_log.compressor_on = compressor_->Enabled(); }
pcm_->Flush();
to_log.read_solenoids = pcm_->GetAll();
@@ -394,10 +415,13 @@
private:
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_shifter_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> fingers_;
::std::unique_ptr<Compressor> compressor_;
::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
+ ::aos::Queue<::y2017_bot3::control_loops::SuperstructureQueue::Output>
+ superstructure_;
::std::atomic<bool> run_{true};
};
@@ -411,6 +435,7 @@
void set_drivetrain_right_victor(::std::unique_ptr<::frc::VictorSP> t) {
drivetrain_right_victor_ = ::std::move(t);
};
+
private:
virtual void Read() override {
::frc971::control_loops::drivetrain_queue.output.FetchAnother();
@@ -433,6 +458,37 @@
drivetrain_right_victor_;
};
+class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_rollers_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ rollers_victor_ = ::std::move(t);
+ }
+
+ void set_hanger_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ hanger_victor_ = ::std::move(t);
+ }
+ private:
+ virtual void Read() override {
+ ::y2017_bot3::control_loops::superstructure_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ 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);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Superstructure output too old.\n");
+ rollers_victor_->SetDisabled();
+ hanger_victor_->SetDisabled();
+ }
+
+ ::std::unique_ptr<::frc::VictorSP> rollers_victor_;
+ ::std::unique_ptr<::frc::VictorSP> hanger_victor_;
+};
+
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
::std::unique_ptr<Encoder> make_encoder(int index) {
@@ -457,7 +513,7 @@
reader.set_drivetrain_left_hall(make_unique<AnalogInput>(0));
reader.set_drivetrain_right_hall(make_unique<AnalogInput>(1));
- reader.set_pwm_trigger(make_unique<DigitalInput>(7));
+ reader.set_pwm_trigger(make_unique<DigitalInput>(0));
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
@@ -466,15 +522,22 @@
DrivetrainWriter drivetrain_writer;
drivetrain_writer.set_drivetrain_left_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(7)));
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)));
drivetrain_writer.set_drivetrain_right_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)));
::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)));
+
::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_fingers(pcm->MakeSolenoid(2));
solenoid_writer.set_compressor(make_unique<Compressor>());