Holds can in auto.
Change-Id: I3d1f9c473b58552cc6b216d88162a42256388ff8
diff --git a/bot3/autonomous/auto.cc b/bot3/autonomous/auto.cc
index 3e770ba..813b1ec 100644
--- a/bot3/autonomous/auto.cc
+++ b/bot3/autonomous/auto.cc
@@ -11,9 +11,13 @@
#include "bot3/autonomous/auto.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "bot3/control_loops/elevator/elevator.q.h"
+#include "bot3/control_loops/intake/intake.q.h"
using ::aos::time::Time;
using ::bot3::control_loops::drivetrain_queue;
+using ::bot3::control_loops::intake_queue;
+using ::bot3::control_loops::elevator_queue;
namespace bot3 {
namespace autonomous {
@@ -64,8 +68,7 @@
while (true) {
autonomous::can_grabber_control.MakeWithBuilder()
- .can_grabber_voltage(voltage)
- .Send();
+ .can_grabber_voltage(voltage).can_grabbers(false).Send();
// Poll the running bit and auto done bits.
if (ShouldExitAuto()) {
@@ -84,20 +87,44 @@
ResetDrivetrain();
// Launch can grabbers.
- GrabberForTime(12.0, 0.26);
+ //GrabberForTime(12.0, 0.26);
+ GrabberForTime(6.0, 0.40);
if (ShouldExitAuto()) return;
InitializeEncoders();
ResetDrivetrain();
if (ShouldExitAuto()) return;
+ // Send our intake goals.
+ if (!intake_queue.goal.MakeWithBuilder().movement(0.0).claw_closed(true)
+ .Send()) {
+ LOG(ERROR, "Sending intake goal failed.\n");
+ }
+ {
+ auto new_elevator_goal = elevator_queue.goal.MakeMessage();
+ new_elevator_goal->max_velocity = 0.0;
+ new_elevator_goal->max_acceleration = 0.0;
+ new_elevator_goal->height = 0.030;
+ new_elevator_goal->velocity = 0.0;
+ new_elevator_goal->passive_support = true;
+ new_elevator_goal->can_support = false;
+
+ if (new_elevator_goal.Send()) {
+ LOG(DEBUG, "sending goals: elevator: %f\n", 0.03);
+ } else {
+ LOG(ERROR, "Sending elevator goal failed.\n");
+ }
+ }
+
+ const double kMoveBackDistance = 1.75;
+ //const double kMoveBackDistance = 0.0;
// Drive backwards, and pulse the can grabbers again to tip the cans.
control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
.steering(0.0)
.throttle(0.0)
- .left_goal(left_initial_position + 1.75)
+ .left_goal(left_initial_position + kMoveBackDistance)
.left_velocity_goal(0)
- .right_goal(right_initial_position + 1.75)
+ .right_goal(right_initial_position + kMoveBackDistance)
.right_velocity_goal(0)
.Send();
GrabberForTime(12.0, 0.02);
diff --git a/bot3/autonomous/auto.q b/bot3/autonomous/auto.q
index d529500..7f2a612 100644
--- a/bot3/autonomous/auto.q
+++ b/bot3/autonomous/auto.q
@@ -9,5 +9,7 @@
message CanGrabberControl {
// Voltage to send out to can grabbers.
double can_grabber_voltage;
+ // Can grabbers fired
+ bool can_grabbers;
};
queue CanGrabberControl can_grabber_control;
diff --git a/bot3/autonomous/autonomous.gyp b/bot3/autonomous/autonomous.gyp
index 7db5959..ed93ad6 100644
--- a/bot3/autonomous/autonomous.gyp
+++ b/bot3/autonomous/autonomous.gyp
@@ -20,6 +20,8 @@
'<(AOS)/common/controls/controls.gyp:control_loop',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_lib',
+ '<(DEPTH)/bot3/control_loops/elevator/elevator.gyp:elevator_queue',
+ '<(DEPTH)/bot3/control_loops/intake/intake.gyp:intake_queue',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/util/util.gyp:phased_loop',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
diff --git a/bot3/joystick_reader.cc b/bot3/joystick_reader.cc
index a819757..2cc555d 100644
--- a/bot3/joystick_reader.cc
+++ b/bot3/joystick_reader.cc
@@ -74,6 +74,7 @@
const ButtonLocation kCanGrabberLift(2, 1);
const ButtonLocation kFastCanGrabberLift(2, 3);
+const ButtonLocation kCanGrabberLower(2, 2);
class Reader : public ::aos::input::JoystickInput {
public:
@@ -89,6 +90,9 @@
} else {
StopAuto();
}
+ intake_closed_ = true;
+ can_restraint_open_ = false;
+ passive_support_open_ = true;
}
if (!data.GetControlBit(ControlBit::kAutonomous)) {
@@ -116,9 +120,6 @@
if (!data.GetControlBit(ControlBit::kEnabled)) {
action_queue_.CancelAllActions();
- intake_closed_ = false;
- can_restraint_open_ = true;
- passive_support_open_ = true;
LOG(DEBUG, "Canceling\n");
}
@@ -208,52 +209,52 @@
action_queue_.CancelAllActions();
}
- if (data.PosEdge(kOpenIntake)) {
+ if (data.IsPressed(kOpenIntake)) {
intake_closed_ = false;
}
- if (data.PosEdge(kCloseIntake)) {
+ if (data.IsPressed(kCloseIntake)) {
intake_closed_ = true;
}
- if (data.PosEdge(kOpenCanRestraint)) {
+ if (data.IsPressed(kOpenCanRestraint)) {
can_restraint_open_ = true;
}
- if (data.PosEdge(kCloseCanRestraint)) {
+ if (data.IsPressed(kCloseCanRestraint)) {
can_restraint_open_ = false;
}
- if (data.PosEdge(kOpenPassiveSupport)) {
+ if (data.IsPressed(kOpenPassiveSupport)) {
passive_support_open_ = true;
}
- if (data.PosEdge(kClosePassiveSupport)) {
+ if (data.IsPressed(kClosePassiveSupport)) {
passive_support_open_ = false;
}
// Buttons for elevator.
- if (data.PosEdge(kCarry)) {
+ if (data.IsPressed(kCarry)) {
// TODO(comran): Get actual height/velocity/acceleration values.
elevator_goal_ = 0.180;
elevator_params_ = {1.0, 2.0};
action_queue_.CancelAllActions();
}
- if (data.PosEdge(kSetDown)) {
+ if (data.IsPressed(kSetDown)) {
// TODO(comran): Get actual height/velocity/acceleration values.
elevator_goal_ = 0.005;
elevator_params_ = {1.0, 5.0};
action_queue_.CancelAllActions();
}
- if (data.PosEdge(kSkyscraper)) {
+ if (data.IsPressed(kSkyscraper)) {
// TODO(comran): Get actual height/velocity/acceleration values.
elevator_goal_ = 1.0;
elevator_params_ = {1.0, 5.0};
}
- if (data.PosEdge(kScoreBegin)) {
+ if (data.IsPressed(kScoreBegin)) {
// TODO(comran): Get actual height/velocity/acceleration values.
elevator_goal_ = 0.030;
elevator_params_ = {1.0, 5.0};
@@ -266,10 +267,13 @@
// Buttons for can grabber.
if (data.IsPressed(kCanGrabberLift)) {
::bot3::autonomous::can_grabber_control.MakeWithBuilder()
- .can_grabber_voltage(-4).Send();
+ .can_grabber_voltage(-4).can_grabbers(false).Send();
} else if (data.IsPressed(kFastCanGrabberLift)) {
::bot3::autonomous::can_grabber_control.MakeWithBuilder()
- .can_grabber_voltage(-12).Send();
+ .can_grabber_voltage(-12).can_grabbers(false).Send();
+ } else if (data.IsPressed(kCanGrabberLower)) {
+ ::bot3::autonomous::can_grabber_control.MakeWithBuilder()
+ .can_grabber_voltage(4).can_grabbers(true).Send();
}
// Send our goals if everything looks OK.
diff --git a/bot3/wpilib/wpilib_interface.cc b/bot3/wpilib/wpilib_interface.cc
index 6926efe..36e14ad 100644
--- a/bot3/wpilib/wpilib_interface.cc
+++ b/bot3/wpilib/wpilib_interface.cc
@@ -201,10 +201,11 @@
// Writes out our pneumatic outputs.
class SolenoidWriter {
public:
- SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
+ SolenoidWriter(const ::std::unique_ptr< ::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
elevator_(".bot3.control_loops.elevator_queue.output"),
- intake_(".bot3.control_loops.intake_queue.output") {}
+ intake_(".bot3.control_loops.intake_queue.output"),
+ can_grabber_control_(".bot3.autonomous.can_grabber_control") {}
void set_pressure_switch(::std::unique_ptr<DigitalSource> pressure_switch) {
pressure_switch_ = ::std::move(pressure_switch);
@@ -219,6 +220,10 @@
elevator_passive_support_ = ::std::move(elevator_passive_support);
}
+ void set_can_grabber(::std::unique_ptr<BufferedSolenoid> can_grabber) {
+ can_grabber_ = ::std::move(can_grabber);
+ }
+
void set_elevator_can_support(
::std::unique_ptr<BufferedSolenoid> elevator_can_support) {
elevator_can_support_ = ::std::move(elevator_can_support);
@@ -235,6 +240,15 @@
while (run_) {
::aos::time::PhasedLoopXMS(20, 1000);
+ // Can Grabber
+ {
+ can_grabber_control_.FetchLatest();
+ if (can_grabber_control_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *can_grabber_control_);
+ can_grabber_->Set(can_grabber_control_->can_grabbers);
+ }
+ }
+
// Elevator
{
elevator_.FetchLatest();
@@ -284,12 +298,14 @@
::std::unique_ptr<BufferedSolenoid> elevator_passive_support_;
::std::unique_ptr<BufferedSolenoid> elevator_can_support_;
::std::unique_ptr<BufferedSolenoid> intake_claw_;
+ ::std::unique_ptr<BufferedSolenoid> can_grabber_;
::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_;
+ ::aos::Queue<::bot3::autonomous::CanGrabberControl> can_grabber_control_;
::std::atomic<bool> run_{true};
};
@@ -491,6 +507,7 @@
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));
+ solenoid_writer.set_can_grabber(pcm->MakeSolenoid(3));
::std::thread solenoid_thread(::std::ref(solenoid_writer));
// Wait forever. Not much else to do...