Added plumbing for piston values through elevator/intake code.
Change-Id: I786fbc1411bc26063a7b03ea5428297317d21daa
diff --git a/bot3/control_loops/elevator/elevator.cc b/bot3/control_loops/elevator/elevator.cc
index 49433a7..a28dd7d 100644
--- a/bot3/control_loops/elevator/elevator.cc
+++ b/bot3/control_loops/elevator/elevator.cc
@@ -237,6 +237,10 @@
if (output) {
output->elevator = loop_->U(0, 0);
+ if(unsafe_goal) {
+ output->passive_support = unsafe_goal->passive_support;
+ output->can_support = unsafe_goal->can_support;
+ }
}
status->zeroed = state_ == RUNNING;
diff --git a/bot3/control_loops/elevator/elevator.q b/bot3/control_loops/elevator/elevator.q
index 35d56f9..6b2ba26 100644
--- a/bot3/control_loops/elevator/elevator.q
+++ b/bot3/control_loops/elevator/elevator.q
@@ -20,6 +20,13 @@
// Maximum elevator profile acceleration or 0 for the default.
float max_acceleration;
+
+ // Whether the passive elevator is supporting the stack/can; true means it
+ // is supporting; false means it is not.
+ bool passive_support;
+ // Whether the can support is restraining the can; true means it
+ // is supporting; false means it is not.
+ bool can_support;
};
message Position {
@@ -58,6 +65,9 @@
// True means support the stack, false means release the support from the
// stack.
bool passive_support;
+ // Toggle for the that supports the can in the robot.
+ // True means support the can, false means release the can.
+ bool can_support;
};
queue Goal goal;
diff --git a/bot3/control_loops/intake/intake.cc b/bot3/control_loops/intake/intake.cc
index 935730b..4788a7a 100644
--- a/bot3/control_loops/intake/intake.cc
+++ b/bot3/control_loops/intake/intake.cc
@@ -19,16 +19,18 @@
const int16_t intake_movement = goal->movement;
- if (intake_movement > 0) {
+ if (intake_movement > 0.0) {
// Suck.
output->intake = kIntakeVoltageFullPower;
- } else if (intake_movement < 0) {
+ } else if (intake_movement < 0.0) {
// Spit.
output->intake = -kIntakeVoltageFullPower;
} else {
// Stationary.
output->intake = 0.0;
}
+
+ output->claw_closed = goal->claw_closed;
}
}
diff --git a/bot3/control_loops/intake/intake.q b/bot3/control_loops/intake/intake.q
index 3047f6b..c09bf87 100644
--- a/bot3/control_loops/intake/intake.q
+++ b/bot3/control_loops/intake/intake.q
@@ -7,13 +7,18 @@
message Goal {
// Positive = suck, negative = spit, zero = stationary.
- int16_t movement;
+ double movement;
+
+ bool claw_closed;
};
message Position {};
message Output {
+ // Positive or negative, depending on whether we're sucking or spitting.
double intake;
+
+ bool claw_closed;
};
message Status {};
diff --git a/bot3/wpilib/wpilib_interface.cc b/bot3/wpilib/wpilib_interface.cc
index 9ea9c58..119c4fc 100644
--- a/bot3/wpilib/wpilib_interface.cc
+++ b/bot3/wpilib/wpilib_interface.cc
@@ -189,7 +189,9 @@
class SolenoidWriter {
public:
SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
- : pcm_(pcm), elevator_(".bot3.control_loops.elevator_queue.output") {}
+ : pcm_(pcm),
+ elevator_(".bot3.control_loops.elevator_queue.output"),
+ intake_(".bot3.control_loops.intake_queue.output") {}
void set_pressure_switch(::std::unique_ptr<DigitalSource> pressure_switch) {
pressure_switch_ = ::std::move(pressure_switch);
@@ -204,6 +206,15 @@
elevator_passive_support_ = ::std::move(elevator_passive_support);
}
+ void set_elevator_can_support(
+ ::std::unique_ptr<BufferedSolenoid> elevator_can_support) {
+ elevator_can_support_ = ::std::move(elevator_can_support);
+ }
+
+ void set_intake_claw(::std::unique_ptr<BufferedSolenoid> intake_claw) {
+ intake_claw_ = ::std::move(intake_claw);
+ }
+
void operator()() {
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(30);
@@ -217,6 +228,16 @@
if (elevator_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *elevator_);
elevator_passive_support_->Set(elevator_->passive_support);
+ elevator_can_support_->Set(elevator_->passive_support);
+ }
+ }
+
+ // Intake
+ {
+ intake_.FetchLatest();
+ if (intake_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *intake_);
+ intake_claw_->Set(intake_->claw_closed);
}
}
@@ -248,11 +269,14 @@
const ::std::unique_ptr<BufferedPcm> &pcm_;
::std::unique_ptr<BufferedSolenoid> elevator_passive_support_;
+ ::std::unique_ptr<BufferedSolenoid> elevator_can_support_;
+ ::std::unique_ptr<BufferedSolenoid> intake_claw_;
::std::unique_ptr<DigitalSource> pressure_switch_;
::std::unique_ptr<Relay> compressor_relay_;
::aos::Queue<::bot3::control_loops::ElevatorQueue::Output> elevator_;
+ ::aos::Queue<::bot3::control_loops::IntakeQueue::Output> intake_;
::std::atomic<bool> run_{true};
};
@@ -395,6 +419,10 @@
SolenoidWriter solenoid_writer(pcm);
solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(9));
solenoid_writer.set_compressor_relay(make_unique<Relay>(0));
+ // TODO (jasmine): Find solenoid numbers
+ solenoid_writer.set_elevator_passive_support(pcm->MakeSolenoid(0));
+ solenoid_writer.set_elevator_can_support(pcm->MakeSolenoid(1));
+ solenoid_writer.set_intake_claw(pcm->MakeSolenoid(2));
::std::thread solenoid_thread(::std::ref(solenoid_writer));
// TODO(comran): Find talon/encoder numbers ^^^