Add can grabber auto.
Change-Id: I47b2c93916018dce3923a8a66c1ac2c663d1f7a1
diff --git a/bot3/autonomous/auto.cc b/bot3/autonomous/auto.cc
index 0a2f99a..3e770ba 100644
--- a/bot3/autonomous/auto.cc
+++ b/bot3/autonomous/auto.cc
@@ -10,7 +10,7 @@
#include "bot3/autonomous/auto.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/actors/drivetrain_actor.h"
+#include "bot3/control_loops/drivetrain/drivetrain.h"
using ::aos::time::Time;
using ::bot3::control_loops::drivetrain_queue;
@@ -25,6 +25,8 @@
namespace time = ::aos::time;
+static double left_initial_position, right_initial_position;
+
bool ShouldExitAuto() {
::bot3::autonomous::autonomous.FetchLatest();
bool ans = !::bot3::autonomous::autonomous->run_auto;
@@ -34,13 +36,90 @@
return ans;
}
+void ResetDrivetrain() {
+ LOG(INFO, "resetting the drivetrain\n");
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .steering(0.0)
+ .throttle(0.0)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
+ .Send();
+}
+
+void InitializeEncoders() {
+ control_loops::drivetrain_queue.status.FetchAnother();
+ left_initial_position =
+ control_loops::drivetrain_queue.status->filtered_left_position;
+ right_initial_position =
+ control_loops::drivetrain_queue.status->filtered_right_position;
+}
+
+void GrabberForTime(double voltage, double wait_time) {
+ ::aos::time::Time now = ::aos::time::Time::Now();
+ ::aos::time::Time end_time = now + time::Time::InSeconds(wait_time);
+ LOG(INFO, "Starting to grab at %f for %f seconds\n", voltage, wait_time);
+
+ while (true) {
+ autonomous::can_grabber_control.MakeWithBuilder()
+ .can_grabber_voltage(voltage)
+ .Send();
+
+ // Poll the running bit and auto done bits.
+ if (ShouldExitAuto()) {
+ return;
+ }
+ if (::aos::time::Time::Now() > end_time) {
+ LOG(INFO, "Done grabbing\n");
+ return;
+ }
+ ::aos::time::PhasedLoopXMS(5, 2500);
+ }
+}
+
+// Auto methods.
+void CanGrabberAuto() {
+ ResetDrivetrain();
+
+ // Launch can grabbers.
+ GrabberForTime(12.0, 0.26);
+ if (ShouldExitAuto()) return;
+ InitializeEncoders();
+ ResetDrivetrain();
+ if (ShouldExitAuto()) return;
+
+ // 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_velocity_goal(0)
+ .right_goal(right_initial_position + 1.75)
+ .right_velocity_goal(0)
+ .Send();
+ GrabberForTime(12.0, 0.02);
+ if (ShouldExitAuto()) return;
+
+ // We shouldn't need as much power at this point, so lower the can grabber
+ // voltages to avoid damaging the motors due to stalling.
+ GrabberForTime(4.0, 0.75);
+ if (ShouldExitAuto()) return;
+ GrabberForTime(-3.0, 0.25);
+ if (ShouldExitAuto()) return;
+ GrabberForTime(-12.0, 1.0);
+ if (ShouldExitAuto()) return;
+ GrabberForTime(-3.0, 12.0);
+}
+
void HandleAuto() {
::aos::time::Time start_time = ::aos::time::Time::Now();
LOG(INFO, "Starting auto mode at %f\n", start_time.ToSeconds());
- ::std::unique_ptr<::frc971::actors::DrivetrainAction> drive;
- LOG(INFO, "Doing nothing in auto.\n");
- if (ShouldExitAuto()) return;
+ // TODO(comran): Add various options for different autos down below.
+ CanGrabberAuto();
}
} // namespace autonomous
diff --git a/bot3/autonomous/auto.q b/bot3/autonomous/auto.q
index 3636d31..d529500 100644
--- a/bot3/autonomous/auto.q
+++ b/bot3/autonomous/auto.q
@@ -4,5 +4,10 @@
// True if auto mode should be running, false otherwise.
bool run_auto;
};
-
queue AutoControl autonomous;
+
+message CanGrabberControl {
+ // Voltage to send out to can grabbers.
+ double can_grabber_voltage;
+};
+queue CanGrabberControl can_grabber_control;
diff --git a/bot3/autonomous/autonomous.gyp b/bot3/autonomous/autonomous.gyp
index 140fb5b..7db5959 100644
--- a/bot3/autonomous/autonomous.gyp
+++ b/bot3/autonomous/autonomous.gyp
@@ -19,11 +19,11 @@
'auto_queue',
'<(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',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/util/util.gyp:phased_loop',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(AOS)/build/aos.gyp:logging',
- #'<(DEPTH)/frc971/actors/actors.gyp:drivetrain_action_lib',
'<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
diff --git a/bot3/joystick_reader.cc b/bot3/joystick_reader.cc
index 9ded2ca..a819757 100644
--- a/bot3/joystick_reader.cc
+++ b/bot3/joystick_reader.cc
@@ -72,6 +72,9 @@
const ButtonLocation kScoreBegin(4, 8);
+const ButtonLocation kCanGrabberLift(2, 1);
+const ButtonLocation kFastCanGrabberLift(2, 3);
+
class Reader : public ::aos::input::JoystickInput {
public:
Reader() : was_running_(false) {}
@@ -230,7 +233,6 @@
}
// Buttons for elevator.
-
if (data.PosEdge(kCarry)) {
// TODO(comran): Get actual height/velocity/acceleration values.
elevator_goal_ = 0.180;
@@ -261,6 +263,15 @@
tote_count_ = 0;
}
+ // Buttons for can grabber.
+ if (data.IsPressed(kCanGrabberLift)) {
+ ::bot3::autonomous::can_grabber_control.MakeWithBuilder()
+ .can_grabber_voltage(-4).Send();
+ } else if (data.IsPressed(kFastCanGrabberLift)) {
+ ::bot3::autonomous::can_grabber_control.MakeWithBuilder()
+ .can_grabber_voltage(-12).Send();
+ }
+
// Send our goals if everything looks OK.
if (!waiting_for_zero_) {
if (!action_queue_.Running()) {
diff --git a/bot3/prime/prime.gyp b/bot3/prime/prime.gyp
index 5772f92..bbc59ad 100644
--- a/bot3/prime/prime.gyp
+++ b/bot3/prime/prime.gyp
@@ -17,7 +17,7 @@
'../control_loops/elevator/elevator.gyp:elevator',
'../control_loops/elevator/elevator.gyp:elevator_lib_test',
#'../control_loops/elevator/elevator.gyp:replay_elevator',
- #'../autonomous/autonomous.gyp:auto_bot3',
+ '../autonomous/autonomous.gyp:auto_bot3',
'../actors/actors.gyp:binaries',
],
'copies': [
diff --git a/bot3/wpilib/wpilib.gyp b/bot3/wpilib/wpilib.gyp
index 2f0e6cd..81f2e9a 100644
--- a/bot3/wpilib/wpilib.gyp
+++ b/bot3/wpilib/wpilib.gyp
@@ -26,6 +26,7 @@
'<(DEPTH)/frc971/wpilib/wpilib.gyp:gyro_sender',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
'<(DEPTH)/frc971/wpilib/wpilib.gyp:logging_queue',
+ '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_lib',
'<(DEPTH)/bot3/control_loops/elevator/elevator.gyp:elevator_lib',
'<(DEPTH)/bot3/control_loops/intake/intake.gyp:intake_lib',
diff --git a/bot3/wpilib/wpilib_interface.cc b/bot3/wpilib/wpilib_interface.cc
index a43a84f..6926efe 100644
--- a/bot3/wpilib/wpilib_interface.cc
+++ b/bot3/wpilib/wpilib_interface.cc
@@ -21,6 +21,7 @@
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "bot3/control_loops/elevator/elevator.q.h"
#include "bot3/control_loops/intake/intake.q.h"
+#include "bot3/autonomous/auto.q.h"
#include "frc971/wpilib/hall_effect.h"
#include "frc971/wpilib/joystick_sender.h"
@@ -167,8 +168,7 @@
auto elevator_message = elevator_queue.position.MakeMessage();
elevator_message->encoder =
elevator_translate(elevator_encoder_->GetRaw());
- elevator_message->bottom_hall_effect =
- zeroing_hall_effect_->Get();
+ elevator_message->bottom_hall_effect = zeroing_hall_effect_->Get();
elevator_message->has_tote = tote_sensor_->GetVoltage() > 2.5;
elevator_message.Send();
@@ -369,7 +369,6 @@
intake_talon2_ = ::std::move(t);
}
-
private:
virtual void Read() override {
::bot3::control_loops::intake_queue.output.FetchAnother();
@@ -392,6 +391,40 @@
::std::unique_ptr<Talon> intake_talon2_;
};
+// Writes out can grabber voltages.
+class CanGrabberWriter : public LoopOutputHandler {
+ public:
+ CanGrabberWriter() : LoopOutputHandler(::aos::time::Time::InSeconds(0.05)) {}
+
+ void set_can_grabber_talon1(::std::unique_ptr<Talon> t) {
+ can_grabber_talon1_ = ::std::move(t);
+ }
+
+ void set_can_grabber_talon2(::std::unique_ptr<Talon> t) {
+ can_grabber_talon2_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::bot3::autonomous::can_grabber_control.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::bot3::autonomous::can_grabber_control;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ can_grabber_talon1_->Set(queue->can_grabber_voltage / 12.0);
+ can_grabber_talon2_->Set(-queue->can_grabber_voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Can grabber output too old\n");
+ can_grabber_talon1_->Disable();
+ can_grabber_talon2_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> can_grabber_talon1_, can_grabber_talon2_;
+};
+
// TODO(brian): Replace this with ::std::make_unique once all our toolchains
// have support.
template <class T, class... U>
@@ -416,8 +449,7 @@
LOG(INFO, "Creating the reader\n");
reader.set_elevator_encoder(encoder(6));
- reader.set_elevator_zeroing_hall_effect(
- make_unique<HallEffect>(6));
+ reader.set_elevator_zeroing_hall_effect(make_unique<HallEffect>(6));
reader.set_left_encoder(encoder(0));
reader.set_right_encoder(encoder(1));
@@ -444,6 +476,13 @@
intake_writer.set_intake_talon2(::std::unique_ptr<Talon>(new Talon(5)));
::std::thread intake_writer_thread(::std::ref(intake_writer));
+ CanGrabberWriter can_grabber_writer;
+ can_grabber_writer.set_can_grabber_talon1(
+ ::std::unique_ptr<Talon>(new Talon(3)));
+ can_grabber_writer.set_can_grabber_talon2(
+ ::std::unique_ptr<Talon>(new Talon(4)));
+ ::std::thread can_grabber_writer_thread(::std::ref(can_grabber_writer));
+
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(pcm);
@@ -475,6 +514,9 @@
intake_writer.Quit();
intake_writer_thread.join();
+ can_grabber_writer.Quit();
+ can_grabber_writer_thread.join();
+
solenoid_writer.Quit();
solenoid_thread.join();