remove all the crio, bbb, and 2014 code
This also meant upgrading to clang-3.5 from llvm.org to try to get it to
build code for the roboRIO.
Change-Id: I44df4af4e9e04296ee7934cc787da3101b1ac449
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 6f13205..86a456b 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -7,7 +7,6 @@
#include <mutex>
#include <functional>
-#include "aos/prime/output/motor_output.h"
#include "aos/common/controls/output_check.q.h"
#include "aos/common/controls/sensor_generation.q.h"
#include "aos/common/logging/logging.h"
@@ -20,11 +19,7 @@
#include "aos/linux_code/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/claw/claw.q.h"
-#include "frc971/control_loops/shooter/shooter.q.h"
#include "frc971/constants.h"
-#include "frc971/queues/other_sensors.q.h"
-#include "frc971/queues/to_log.q.h"
#include "frc971/shifter_hall_effect.h"
#include "frc971/wpilib/hall_effect.h"
@@ -352,331 +347,29 @@
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
-static const double kVcc = 5.15;
-
-float hall_translate(const constants::ShifterHallEffect &k, float in_low,
- float in_high) {
- const float low_ratio =
- 0.5 * (in_low - static_cast<float>(k.low_gear_low)) /
- static_cast<float>(k.low_gear_middle - k.low_gear_low);
- const float high_ratio =
- 0.5 + 0.5 * (in_high - static_cast<float>(k.high_gear_middle)) /
- static_cast<float>(k.high_gear_high - k.high_gear_middle);
-
- // Return low when we are below 1/2, and high when we are above 1/2.
- if (low_ratio + high_ratio < 1.0) {
- return low_ratio;
- } else {
- return high_ratio;
- }
-}
-
-double claw_translate(int32_t in) {
- return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) /
- (18.0 / 48.0 /*encoder gears*/) / (12.0 / 60.0 /*chain reduction*/) *
- (M_PI / 180.0) *
- 2.0 /*TODO(austin): Debug this, encoders read too little*/;
-}
-
-double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- 16 /*sprocket teeth*/ * 0.375 /*chain pitch*/
- * (2.54 / 100.0 /*in to m*/);
-}
-
-// This class sends out half of the claw position state at 100 hz.
-class HalfClawHallSynchronizer : public PeriodicHallSynchronizer<3> {
- public:
- // Constructs a HalfClawHallSynchronizer.
- //
- // priority is the priority of the polling thread.
- // interrupt_priority is the priority of the interrupt threads.
- // encoder is the encoder to read.
- // sensors is an array of hall effect sensors. The sensors[0] is the front
- // sensor, sensors[1] is the calibration sensor, sensors[2] is the back
- // sensor.
- HalfClawHallSynchronizer(
- const char *name, int priority, int interrupt_priority,
- ::std::unique_ptr<Encoder> encoder,
- ::std::array<::std::unique_ptr<HallEffect>, 3> *sensors, bool reversed)
- : PeriodicHallSynchronizer<3>(name, priority, interrupt_priority,
- ::std::move(encoder), sensors),
- reversed_(reversed) {}
-
- void set_position(control_loops::HalfClawPosition *half_claw_position) {
- half_claw_position_ = half_claw_position;
- }
-
- // Saves the state so that it can be sent if it was synchronized.
- virtual void SaveState() {
- const auto &front = edge_counters()[0];
- half_claw_position_->front.current = front->polled_value();
- half_claw_position_->front.posedge_count =
- front->positive_interrupt_count();
- half_claw_position_->front.negedge_count =
- front->negative_interrupt_count();
-
- const auto &calibration = edge_counters()[1];
- half_claw_position_->calibration.current = calibration->polled_value();
- half_claw_position_->calibration.posedge_count =
- calibration->positive_interrupt_count();
- half_claw_position_->calibration.negedge_count =
- calibration->negative_interrupt_count();
-
- const auto &back = edge_counters()[2];
- half_claw_position_->back.current = back->polled_value();
- half_claw_position_->back.posedge_count = back->positive_interrupt_count();
- half_claw_position_->back.negedge_count = back->negative_interrupt_count();
-
- const double multiplier = reversed_ ? -1.0 : 1.0;
-
- half_claw_position_->position =
- multiplier * claw_translate(encoder_value());
-
- // We assume here that we can only have 1 sensor have a posedge per cycle.
- {
- half_claw_position_->posedge_value =
- last_half_claw_position_.posedge_value;
- int posedge_changes = 0;
- if (half_claw_position_->front.posedge_count !=
- last_half_claw_position_.front.posedge_count) {
- ++posedge_changes;
- half_claw_position_->posedge_value =
- multiplier * claw_translate(front->last_positive_encoder_value());
- LOG(INFO, "Got a front posedge\n");
- }
-
- if (half_claw_position_->back.posedge_count !=
- last_half_claw_position_.back.posedge_count) {
- ++posedge_changes;
- half_claw_position_->posedge_value =
- multiplier * claw_translate(back->last_positive_encoder_value());
- LOG(INFO, "Got a back posedge\n");
- }
-
- if (half_claw_position_->calibration.posedge_count !=
- last_half_claw_position_.calibration.posedge_count) {
- ++posedge_changes;
- half_claw_position_->posedge_value =
- multiplier *
- claw_translate(calibration->last_positive_encoder_value());
- LOG(INFO, "Got a calibration posedge\n");
- }
-
- if (posedge_changes > 1) {
- LOG(WARNING, "Found more than 1 positive edge on %s\n", name());
- }
- }
-
- {
- half_claw_position_->negedge_value =
- last_half_claw_position_.negedge_value;
- int negedge_changes = 0;
- if (half_claw_position_->front.negedge_count !=
- last_half_claw_position_.front.negedge_count) {
- ++negedge_changes;
- half_claw_position_->negedge_value =
- multiplier * claw_translate(front->last_negative_encoder_value());
- LOG(INFO, "Got a front negedge\n");
- }
-
- if (half_claw_position_->back.negedge_count !=
- last_half_claw_position_.back.negedge_count) {
- ++negedge_changes;
- half_claw_position_->negedge_value =
- multiplier * claw_translate(back->last_negative_encoder_value());
- LOG(INFO, "Got a back negedge\n");
- }
-
- if (half_claw_position_->calibration.negedge_count !=
- last_half_claw_position_.calibration.negedge_count) {
- ++negedge_changes;
- half_claw_position_->negedge_value =
- multiplier *
- claw_translate(calibration->last_negative_encoder_value());
- LOG(INFO, "Got a calibration negedge\n");
- }
-
- if (negedge_changes > 1) {
- LOG(WARNING, "Found more than 1 negative edge on %s\n", name());
- }
- }
-
- last_half_claw_position_ = *half_claw_position_;
- }
-
- private:
- control_loops::HalfClawPosition *half_claw_position_;
- control_loops::HalfClawPosition last_half_claw_position_;
- bool reversed_;
-};
-
-// This class sends out the shooter position state at 100 hz.
-class ShooterHallSynchronizer : public PeriodicHallSynchronizer<2> {
- public:
- // Constructs a ShooterHallSynchronizer.
- //
- // priority is the priority of the polling thread.
- // interrupt_priority is the priority of the interrupt threads.
- // encoder is the encoder to read.
- // sensors is an array of hall effect sensors. The sensors[0] is the proximal
- // sensor, sensors[1] is the distal sensor.
- // shooter_plunger is the plunger.
- // shooter_latch is the latch.
- ShooterHallSynchronizer(
- int priority, int interrupt_priority, ::std::unique_ptr<Encoder> encoder,
- ::std::array<::std::unique_ptr<HallEffect>, 2> *sensors,
- ::std::unique_ptr<HallEffect> shooter_plunger,
- ::std::unique_ptr<HallEffect> shooter_latch)
- : PeriodicHallSynchronizer<2>("shooter", priority, interrupt_priority,
- ::std::move(encoder), sensors),
- shooter_plunger_(::std::move(shooter_plunger)),
- shooter_latch_(::std::move(shooter_latch)) {}
-
- // Saves the state so that it can be sent if it was synchronized.
- virtual void SaveState() {
- auto shooter_position =
- control_loops::shooter_queue_group.position.MakeMessage();
-
- shooter_position->plunger = shooter_plunger_->GetHall();
- shooter_position->latch = shooter_latch_->GetHall();
- shooter_position->position = shooter_translate(encoder_value());
-
- {
- const auto &proximal_edge_counter = edge_counters()[0];
- shooter_position->pusher_proximal.current =
- proximal_edge_counter->polled_value();
- shooter_position->pusher_proximal.posedge_count =
- proximal_edge_counter->positive_interrupt_count();
- shooter_position->pusher_proximal.negedge_count =
- proximal_edge_counter->negative_interrupt_count();
- shooter_position->pusher_proximal.posedge_value = shooter_translate(
- proximal_edge_counter->last_positive_encoder_value());
- }
-
- {
- const auto &distal_edge_counter = edge_counters()[1];
- shooter_position->pusher_distal.current =
- distal_edge_counter->polled_value();
- shooter_position->pusher_distal.posedge_count =
- distal_edge_counter->positive_interrupt_count();
- shooter_position->pusher_distal.negedge_count =
- distal_edge_counter->negative_interrupt_count();
- shooter_position->pusher_distal.posedge_value =
- shooter_translate(distal_edge_counter->last_positive_encoder_value());
- }
-
- shooter_position.Send();
- }
-
- private:
- ::std::unique_ptr<HallEffect> shooter_plunger_;
- ::std::unique_ptr<HallEffect> shooter_latch_;
-};
-
class SensorReader {
public:
SensorReader()
- : auto_selector_analog_(new AnalogInput(4)),
- left_encoder_(new Encoder(11, 10, false, Encoder::k2X)), // E0
+ : left_encoder_(new Encoder(11, 10, false, Encoder::k2X)), // E0
right_encoder_(new Encoder(13, 12, false, Encoder::k2X)), // E1
- low_left_drive_hall_(new AnalogInput(1)),
- high_left_drive_hall_(new AnalogInput(0)),
- low_right_drive_hall_(new AnalogInput(2)),
- high_right_drive_hall_(new AnalogInput(3)),
- shooter_plunger_(new HallEffect(8)), // S3
- shooter_latch_(new HallEffect(9)), // S4
- shooter_distal_(new HallEffect(7)), // S2
- shooter_proximal_(new HallEffect(6)), // S1
- shooter_encoder_(new Encoder(14, 15)), // E2
- claw_top_front_hall_(new HallEffect(4)), // R2
- claw_top_calibration_hall_(new HallEffect(3)), // R3
- claw_top_back_hall_(new HallEffect(5)), // R1
- claw_top_encoder_(new Encoder(17, 16)), // E3
- // L2 Middle Left hall effect sensor.
- claw_bottom_front_hall_(new HallEffect(1)),
- // L3 Bottom Left hall effect sensor
- claw_bottom_calibration_hall_(new HallEffect(0)),
- // L1 Top Left hall effect sensor
- claw_bottom_back_hall_(new HallEffect(2)),
- claw_bottom_encoder_(new Encoder(21, 20)), // E5
run_(true) {
filter_.SetPeriodNanoSeconds(100000);
- filter_.Add(shooter_plunger_.get());
- filter_.Add(shooter_latch_.get());
- filter_.Add(shooter_distal_.get());
- filter_.Add(shooter_proximal_.get());
- filter_.Add(claw_top_front_hall_.get());
- filter_.Add(claw_top_calibration_hall_.get());
- filter_.Add(claw_top_back_hall_.get());
- filter_.Add(claw_bottom_front_hall_.get());
- filter_.Add(claw_bottom_calibration_hall_.get());
- filter_.Add(claw_bottom_back_hall_.get());
}
void operator()() {
::aos::SetCurrentThreadName("SensorReader");
const int kPriority = 30;
- const int kInterruptPriority = 55;
-
- ::std::array<::std::unique_ptr<HallEffect>, 2> shooter_sensors;
- shooter_sensors[0] = ::std::move(shooter_proximal_);
- shooter_sensors[1] = ::std::move(shooter_distal_);
- ShooterHallSynchronizer shooter(
- kPriority, kInterruptPriority, ::std::move(shooter_encoder_),
- &shooter_sensors, ::std::move(shooter_plunger_),
- ::std::move(shooter_latch_));
- shooter.StartThread();
-
- ::std::array<::std::unique_ptr<HallEffect>, 3> claw_top_sensors;
- claw_top_sensors[0] = ::std::move(claw_top_front_hall_);
- claw_top_sensors[1] = ::std::move(claw_top_calibration_hall_);
- claw_top_sensors[2] = ::std::move(claw_top_back_hall_);
- HalfClawHallSynchronizer top_claw("top_claw", kPriority, kInterruptPriority,
- ::std::move(claw_top_encoder_),
- &claw_top_sensors, false);
-
- ::std::array<::std::unique_ptr<HallEffect>, 3> claw_bottom_sensors;
- claw_bottom_sensors[0] = ::std::move(claw_bottom_front_hall_);
- claw_bottom_sensors[1] = ::std::move(claw_bottom_calibration_hall_);
- claw_bottom_sensors[2] = ::std::move(claw_bottom_back_hall_);
- HalfClawHallSynchronizer bottom_claw(
- "bottom_claw", kPriority, kInterruptPriority,
- ::std::move(claw_bottom_encoder_), &claw_bottom_sensors, true);
+ //const int kInterruptPriority = 55;
::aos::SetCurrentThreadRealtimePriority(kPriority);
while (run_) {
- ::aos::time::PhasedLoopXMS(10, 9000);
+ ::aos::time::PhasedLoopXMS(5, 9000);
RunIteration();
-
- auto claw_position =
- control_loops::claw_queue_group.position.MakeMessage();
- bottom_claw.set_position(&claw_position->bottom);
- top_claw.set_position(&claw_position->top);
- while (true) {
- bottom_claw.StartIteration();
- top_claw.StartIteration();
-
- // Wait more than the amount of time it takes for a digital input change
- // to go from visible to software to having triggered an interrupt.
- ::aos::time::SleepFor(::aos::time::Time::InUS(120));
-
- if (bottom_claw.TryFinishingIteration() &&
- top_claw.TryFinishingIteration()) {
- break;
- }
- }
-
- claw_position.Send();
}
- shooter.Quit();
- top_claw.Quit();
- bottom_claw.Quit();
}
void RunIteration() {
- //::aos::time::TimeFreezer time_freezer;
DriverStation *ds = DriverStation::GetInstance();
if (ds->IsSysActive()) {
@@ -688,22 +381,13 @@
message.Send();
}
- ::frc971::sensors::auto_mode.MakeWithBuilder()
- .voltage(auto_selector_analog_->GetVoltage())
- .Send();
-
// TODO(austin): Calibrate the shifter constants again.
+ // TODO(sensors): Hook up the new dog position sensors.
drivetrain.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(right_encoder_->GetRaw()))
.left_encoder(-drivetrain_translate(left_encoder_->GetRaw()))
- .left_shifter_position(
- hall_translate(constants::GetValues().left_drive,
- low_left_drive_hall_->GetVoltage(),
- high_left_drive_hall_->GetVoltage()))
- .right_shifter_position(
- hall_translate(constants::GetValues().right_drive,
- low_right_drive_hall_->GetVoltage(),
- high_right_drive_hall_->GetVoltage()))
+ .left_shifter_position(0)
+ .right_shifter_position(0)
.battery_voltage(ds->GetBatteryVoltage())
.Send();
@@ -721,26 +405,6 @@
::std::unique_ptr<Encoder> left_encoder_;
::std::unique_ptr<Encoder> right_encoder_;
- ::std::unique_ptr<AnalogInput> low_left_drive_hall_;
- ::std::unique_ptr<AnalogInput> high_left_drive_hall_;
- ::std::unique_ptr<AnalogInput> low_right_drive_hall_;
- ::std::unique_ptr<AnalogInput> high_right_drive_hall_;
-
- ::std::unique_ptr<HallEffect> shooter_plunger_;
- ::std::unique_ptr<HallEffect> shooter_latch_;
- ::std::unique_ptr<HallEffect> shooter_distal_;
- ::std::unique_ptr<HallEffect> shooter_proximal_;
- ::std::unique_ptr<Encoder> shooter_encoder_;
-
- ::std::unique_ptr<HallEffect> claw_top_front_hall_;
- ::std::unique_ptr<HallEffect> claw_top_calibration_hall_;
- ::std::unique_ptr<HallEffect> claw_top_back_hall_;
- ::std::unique_ptr<Encoder> claw_top_encoder_;
-
- ::std::unique_ptr<HallEffect> claw_bottom_front_hall_;
- ::std::unique_ptr<HallEffect> claw_bottom_calibration_hall_;
- ::std::unique_ptr<HallEffect> claw_bottom_back_hall_;
- ::std::unique_ptr<Encoder> claw_bottom_encoder_;
::std::atomic<bool> run_;
DigitalGlitchFilter filter_;
@@ -749,9 +413,7 @@
class SolenoidWriter {
public:
SolenoidWriter(const ::std::unique_ptr<BufferedPcm> &pcm)
- : pcm_(pcm),
- drivetrain_(".frc971.control_loops.drivetrain.output"),
- shooter_(".frc971.control_loops.shooter_queue_group.output") {}
+ : pcm_(pcm), drivetrain_(".frc971.control_loops.drivetrain.output") {}
void set_drivetrain_left(::std::unique_ptr<BufferedSolenoid> s) {
drivetrain_left_ = ::std::move(s);
@@ -761,14 +423,6 @@
drivetrain_right_ = ::std::move(s);
}
- void set_shooter_latch(::std::unique_ptr<BufferedSolenoid> s) {
- shooter_latch_ = ::std::move(s);
- }
-
- void set_shooter_brake(::std::unique_ptr<BufferedSolenoid> s) {
- shooter_brake_ = ::std::move(s);
- }
-
void operator()() {
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(30);
@@ -785,15 +439,6 @@
}
}
- {
- shooter_.FetchLatest();
- if (shooter_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *shooter_);
- shooter_latch_->Set(!shooter_->latch_piston);
- shooter_brake_->Set(!shooter_->brake_piston);
- }
- }
-
pcm_->Flush();
}
}
@@ -804,11 +449,8 @@
const ::std::unique_ptr<BufferedPcm> &pcm_;
::std::unique_ptr<BufferedSolenoid> drivetrain_left_;
::std::unique_ptr<BufferedSolenoid> drivetrain_right_;
- ::std::unique_ptr<BufferedSolenoid> shooter_latch_;
- ::std::unique_ptr<BufferedSolenoid> shooter_brake_;
::aos::Queue<::frc971::control_loops::Drivetrain::Output> drivetrain_;
- ::aos::Queue<::frc971::control_loops::ShooterGroup::Output> shooter_;
::std::atomic<bool> run_{true};
};
@@ -845,91 +487,6 @@
::std::unique_ptr<Talon> right_drivetrain_talon_;
};
-class ShooterWriter : public LoopOutputHandler {
- public:
- void set_shooter_talon(::std::unique_ptr<Talon> t) {
- shooter_talon_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::shooter_queue_group.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::shooter_queue_group.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- shooter_talon_->Set(queue->voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "shooter output too old\n");
- shooter_talon_->Disable();
- }
-
- ::std::unique_ptr<Talon> shooter_talon_;
-};
-
-class ClawWriter : public LoopOutputHandler {
- public:
- void set_top_claw_talon(::std::unique_ptr<Talon> t) {
- top_claw_talon_ = ::std::move(t);
- }
-
- void set_bottom_claw_talon(::std::unique_ptr<Talon> t) {
- bottom_claw_talon_ = ::std::move(t);
- }
-
- void set_left_tusk_talon(::std::unique_ptr<Talon> t) {
- left_tusk_talon_ = ::std::move(t);
- }
-
- void set_right_tusk_talon(::std::unique_ptr<Talon> t) {
- right_tusk_talon_ = ::std::move(t);
- }
-
- void set_intake1_talon(::std::unique_ptr<Talon> t) {
- intake1_talon_ = ::std::move(t);
- }
-
- void set_intake2_talon(::std::unique_ptr<Talon> t) {
- intake2_talon_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::claw_queue_group.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::claw_queue_group.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- intake1_talon_->Set(queue->intake_voltage / 12.0);
- intake2_talon_->Set(queue->intake_voltage / 12.0);
- bottom_claw_talon_->Set(-queue->bottom_claw_voltage / 12.0);
- top_claw_talon_->Set(queue->top_claw_voltage / 12.0);
- left_tusk_talon_->Set(queue->tusk_voltage / 12.0);
- right_tusk_talon_->Set(-queue->tusk_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "claw output too old\n");
- intake1_talon_->Disable();
- intake2_talon_->Disable();
- bottom_claw_talon_->Disable();
- top_claw_talon_->Disable();
- left_tusk_talon_->Disable();
- right_tusk_talon_->Disable();
- }
-
- ::std::unique_ptr<Talon> top_claw_talon_;
- ::std::unique_ptr<Talon> bottom_claw_talon_;
- ::std::unique_ptr<Talon> left_tusk_talon_;
- ::std::unique_ptr<Talon> right_tusk_talon_;
- ::std::unique_ptr<Talon> intake1_talon_;
- ::std::unique_ptr<Talon> intake2_talon_;
-};
-
} // namespace wpilib
} // namespace frc971
@@ -955,26 +512,11 @@
::std::unique_ptr<Talon>(new Talon(2)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- ::frc971::wpilib::ClawWriter claw_writer;
- claw_writer.set_top_claw_talon(::std::unique_ptr<Talon>(new Talon(1)));
- claw_writer.set_bottom_claw_talon(::std::unique_ptr<Talon>(new Talon(0)));
- claw_writer.set_left_tusk_talon(::std::unique_ptr<Talon>(new Talon(4)));
- claw_writer.set_right_tusk_talon(::std::unique_ptr<Talon>(new Talon(3)));
- claw_writer.set_intake1_talon(::std::unique_ptr<Talon>(new Talon(7)));
- claw_writer.set_intake2_talon(::std::unique_ptr<Talon>(new Talon(8)));
- ::std::thread claw_writer_thread(::std::ref(claw_writer));
-
- ::frc971::wpilib::ShooterWriter shooter_writer;
- shooter_writer.set_shooter_talon(::std::unique_ptr<Talon>(new Talon(6)));
- ::std::thread shooter_writer_thread(::std::ref(shooter_writer));
-
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
::frc971::wpilib::SolenoidWriter solenoid_writer(pcm);
solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(6));
solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(7));
- solenoid_writer.set_shooter_latch(pcm->MakeSolenoid(5));
- solenoid_writer.set_shooter_brake(pcm->MakeSolenoid(4));
::std::thread solenoid_thread(::std::ref(solenoid_writer));
// Wait forever. Not much else to do...
@@ -991,10 +533,6 @@
drivetrain_writer.Quit();
drivetrain_writer_thread.join();
- claw_writer.Quit();
- claw_writer_thread.join();
- shooter_writer.Quit();
- shooter_writer_thread.join();
solenoid_writer.Quit();
solenoid_thread.join();