Added cangrabber auto.
Change-Id: I67c2b363df5fc3e431acf0cfd56c9537bb89804b
diff --git a/frc971/wpilib/loop_output_handler.cc b/frc971/wpilib/loop_output_handler.cc
index cb5e4ec..658b064 100644
--- a/frc971/wpilib/loop_output_handler.cc
+++ b/frc971/wpilib/loop_output_handler.cc
@@ -11,9 +11,8 @@
namespace frc971 {
namespace wpilib {
-constexpr ::aos::time::Time LoopOutputHandler::kStopTimeout;
-
-LoopOutputHandler::LoopOutputHandler() : watchdog_(this) {}
+LoopOutputHandler::LoopOutputHandler(const ::aos::time::Time &timeout)
+ : watchdog_(this, timeout) {}
void LoopOutputHandler::operator()() {
::aos::SetCurrentThreadName("OutputHandler");
@@ -44,8 +43,10 @@
watchdog_thread.join();
}
-LoopOutputHandler::Watchdog::Watchdog(LoopOutputHandler *handler)
+LoopOutputHandler::Watchdog::Watchdog(LoopOutputHandler *handler,
+ const ::aos::time::Time &timeout)
: handler_(handler),
+ timeout_(timeout),
timerfd_(timerfd_create(::aos::time::Time::kDefaultClock, 0)) {
if (timerfd_.get() == -1) {
PLOG(FATAL, "timerfd_create(Time::kDefaultClock, 0)");
@@ -64,7 +65,7 @@
void LoopOutputHandler::Watchdog::Reset() {
itimerspec value = itimerspec();
- value.it_value = kStopTimeout.ToTimespec();
+ value.it_value = timeout_.ToTimespec();
PCHECK(timerfd_settime(timerfd_.get(), 0, &value, nullptr));
}
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index fe32763..d7b6283 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -20,7 +20,8 @@
// loops writing values until Quit() is called.
class LoopOutputHandler {
public:
- LoopOutputHandler();
+ LoopOutputHandler(
+ const ::aos::time::Time &timeout = ::aos::time::Time::InSeconds(0.10));
void Quit() { run_ = false; }
@@ -52,7 +53,7 @@
// LoopOutputHandler whenever the timerfd expires.
class Watchdog {
public:
- Watchdog(LoopOutputHandler *handler);
+ Watchdog(LoopOutputHandler *handler, const ::aos::time::Time &timeout);
void operator()();
@@ -63,14 +64,13 @@
private:
LoopOutputHandler *const handler_;
+ const ::aos::time::Time timeout_;
+
::aos::ScopedFD timerfd_;
::std::atomic<bool> run_{true};
};
- static constexpr ::aos::time::Time kStopTimeout =
- ::aos::time::Time::InSeconds(0.02);
-
Watchdog watchdog_;
::std::atomic<bool> run_{true};
diff --git a/y2015/autonomous/auto.cc b/y2015/autonomous/auto.cc
index 9ade207..4e8aa59 100644
--- a/y2015/autonomous/auto.cc
+++ b/y2015/autonomous/auto.cc
@@ -9,6 +9,7 @@
#include "aos/common/logging/queue_logging.h"
#include "frc971/autonomous/auto.q.h"
+#include "y2015/autonomous/auto.q.h"
#include "y2015/constants.h"
#include "y2015/control_loops/drivetrain/drivetrain.q.h"
#include "y2015/actors/drivetrain_actor.h"
@@ -306,9 +307,7 @@
}
}
-void HandleAuto() {
- ::aos::time::Time start_time = ::aos::time::Time::Now();
- LOG(INFO, "Starting auto mode at %f\n", start_time.ToSeconds());
+void TripleCanAuto() {
::std::unique_ptr<::frc971::actors::DrivetrainAction> drive;
::std::unique_ptr<::frc971::actors::PickupAction> pickup;
::std::unique_ptr<::frc971::actors::StackAction> stack;
@@ -554,5 +553,56 @@
if (ShouldExitAuto()) return;
}
+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_control.MakeWithBuilder().can_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);
+ }
+}
+
+void CanGrabberAuto() {
+ ResetDrivetrain();
+ GrabberForTime(12.0, 0.18);
+ if (ShouldExitAuto()) return;
+
+ //GrabberForTime(4.0, 0.10);
+ if (ShouldExitAuto()) return;
+ InitializeEncoders();
+ ResetDrivetrain();
+ if (ShouldExitAuto()) return;
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ //.highgear(false)
+ .steering(0.0)
+ .throttle(0.0)
+ .left_goal(left_initial_position + 1.5)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position + 1.5)
+ .right_velocity_goal(0)
+ .Send();
+ GrabberForTime(12.0, 0.02);
+
+ GrabberForTime(4.0, 14.0);
+ if (ShouldExitAuto()) return;
+}
+
+void HandleAuto() {
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
+ LOG(INFO, "Starting auto mode at %f\n", start_time.ToSeconds());
+ //TripleCanAuto();
+ CanGrabberAuto();
+}
+
} // namespace autonomous
} // namespace frc971
diff --git a/y2015/autonomous/autonomous.gyp b/y2015/autonomous/autonomous.gyp
index 63898e8..f514a81 100644
--- a/y2015/autonomous/autonomous.gyp
+++ b/y2015/autonomous/autonomous.gyp
@@ -22,6 +22,7 @@
'<(DEPTH)/y2015/actors/actors.gyp:stack_action_lib',
'<(DEPTH)/y2015/actors/actors.gyp:held_to_lift_action_lib',
'<(DEPTH)/y2015/actors/actors.gyp:pickup_action_lib',
+ ':auto_queue',
],
'export_dependent_settings': [
'<(AOS)/common/controls/controls.gyp:control_loop',
@@ -39,5 +40,14 @@
'auto_lib',
],
},
+ {
+ 'target_name': 'auto_queue',
+ 'type': 'static_library',
+ 'sources': ['auto.q'],
+ 'variables': {
+ 'header_path': 'y2015/autonomous',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
],
}
diff --git a/y2015/joystick_reader.cc b/y2015/joystick_reader.cc
index 01b5431..84f285b 100644
--- a/y2015/joystick_reader.cc
+++ b/y2015/joystick_reader.cc
@@ -17,6 +17,7 @@
#include "y2015/constants.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
+#include "y2015/autonomous/auto.q.h"
#include "y2015/actors/pickup_actor.h"
#include "y2015/actors/stack_actor.h"
#include "y2015/actors/score_actor.h"
@@ -100,6 +101,8 @@
const ButtonLocation kCoopBottomRetract(4, 12);
const ButtonLocation kCanReset(3, 9);
+const ButtonLocation kCanGrabberLift(2, 1);
+const ButtonLocation kFastCanGrabberLift(2, 3);
const POVLocation kFridgeToggle(4, 270);
const ButtonLocation kSpit(4, 3);
@@ -198,6 +201,11 @@
action_queue_.CancelAllActions();
LOG(DEBUG, "Canceling\n");
}
+ if (data.IsPressed(kCanGrabberLift)) {
+ autonomous::can_control.MakeWithBuilder().can_voltage(-4).Send();
+ } else if (data.IsPressed(kFastCanGrabberLift)) {
+ autonomous::can_control.MakeWithBuilder().can_voltage(-12).Send();
+ }
if (data.IsPressed(kRollersIn)) {
intake_power = 10.0;
@@ -231,7 +239,7 @@
params.spit_time = 0.01;
params.spit_power = -8.0;
- params.suck_time = 0.10;
+ params.suck_time = 0.040;
params.suck_power = 10.0;
params.claw_settle_time = 0.05;
diff --git a/y2015/wpilib/wpilib.gyp b/y2015/wpilib/wpilib.gyp
index 8c1be39..74e45bd 100644
--- a/y2015/wpilib/wpilib.gyp
+++ b/y2015/wpilib/wpilib.gyp
@@ -15,6 +15,7 @@
'<(DEPTH)/y2015/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(DEPTH)/y2015/control_loops/fridge/fridge.gyp:fridge_queue',
'<(DEPTH)/y2015/control_loops/claw/claw.gyp:claw_queue',
+ '<(DEPTH)/y2015/autonomous/autonomous.gyp:auto_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
'<(AOS)/common/util/util.gyp:log_interval',
'<(AOS)/common/common.gyp:time',
diff --git a/y2015/wpilib/wpilib_interface.cc b/y2015/wpilib/wpilib_interface.cc
index ff836a1..758ecf1 100644
--- a/y2015/wpilib/wpilib_interface.cc
+++ b/y2015/wpilib/wpilib_interface.cc
@@ -22,6 +22,7 @@
#include "y2015/control_loops/drivetrain/drivetrain.q.h"
#include "y2015/control_loops/fridge/fridge.q.h"
#include "y2015/control_loops/claw/claw.q.h"
+#include "y2015/autonomous/auto.q.h"
#include "frc971/wpilib/hall_effect.h"
#include "frc971/wpilib/joystick_sender.h"
@@ -477,6 +478,33 @@
::std::atomic<bool> run_{true};
};
+class CanWriter : public LoopOutputHandler {
+ public:
+ CanWriter() : LoopOutputHandler(::aos::time::Time::InSeconds(0.10)) {}
+
+ void set_can_talon(::std::unique_ptr<Talon> t) {
+ can_talon_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::frc971::autonomous::can_control.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::frc971::autonomous::can_control;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ can_talon_->Set(queue->can_voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Can output too old\n");
+ can_talon_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> can_talon_;
+};
+
class DrivetrainWriter : public LoopOutputHandler {
public:
void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
@@ -651,6 +679,10 @@
::std::unique_ptr<Talon>(new Talon(0)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+ CanWriter can_writer;
+ can_writer.set_can_talon(::std::unique_ptr<Talon>(new Talon(9)));
+ ::std::thread can_writer_thread(::std::ref(can_writer));
+
// TODO(sensors): Get real PWM output and relay numbers for the fridge and
// claw.
FridgeWriter fridge_writer;
@@ -702,6 +734,8 @@
drivetrain_writer.Quit();
drivetrain_writer_thread.join();
+ can_writer.Quit();
+ can_writer_thread.join();
solenoid_writer.Quit();
solenoid_thread.join();
diff --git a/y2015/y2015.gyp b/y2015/y2015.gyp
index 7f929d9..8abafb8 100644
--- a/y2015/y2015.gyp
+++ b/y2015/y2015.gyp
@@ -36,6 +36,7 @@
'<(DEPTH)/y2015/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(DEPTH)/y2015/control_loops/fridge/fridge.gyp:fridge_queue',
'<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/y2015/autonomous/autonomous.gyp:auto_queue',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
'<(DEPTH)/y2015/actors/actors.gyp:stack_action_lib',
'<(DEPTH)/y2015/actors/actors.gyp:stack_and_lift_action_lib',