Add changes to joystick_reader for third robot.
These were initially made by Natalia, with a lot of cleaning
up and additional changes by me.
(This was formerly a part of I3d012fb8a9652c0b85ed27f5d23fe7d63bb977ce.)
Change-Id: I23e19b7458dd8dba3b695b456917925d732c5606
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
index 9156509..b8ac058 100644
--- a/bot3/input/joystick_reader.cc
+++ b/bot3/input/joystick_reader.cc
@@ -10,227 +10,39 @@
#include "aos/common/util/log_interval.h"
#include "aos/common/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/constants.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
+#include "bot3/control_loops/rollers/rollers.q.h"
#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/claw/claw.q.h"
-#include "frc971/control_loops/shooter/shooter.q.h"
-#include "frc971/actions/shoot_action.q.h"
-#include "frc971/actions/action_client.h"
-#include "frc971/actions/catch_action.q.h"
-#include "frc971/actions/shoot_action.h"
+#include "frc971/queues/other_sensors.q.h"
-using ::frc971::control_loops::drivetrain;
+using ::bot3::control_loops::drivetrain;
using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::ControlBit;
-#define OLD_DS 0
-
-namespace frc971 {
+namespace bot3 {
namespace input {
namespace joysticks {
const ButtonLocation kDriveControlLoopEnable1(1, 7),
kDriveControlLoopEnable2(1, 11);
-const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(4, 2);
+const ButtonLocation kShiftHigh(4, 1), kShiftLow(4, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kCatch(3, 10);
-
-#if OLD_DS
-const ButtonLocation kFire(3, 11);
-const ButtonLocation kUnload(1, 4);
-const ButtonLocation kReload(1, 2);
-
-const ButtonLocation kRollersOut(3, 12);
-const ButtonLocation kRollersIn(3, 7);
-
-const ButtonLocation kTuck(3, 9);
-const ButtonLocation kIntakePosition(3, 8);
-const ButtonLocation kIntakeOpenPosition(3, 10);
-const ButtonLocation kVerticalTuck(3, 1);
-const JoystickAxis kFlipRobot(3, 3);
-
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kCloseShot(3, 2);
-const ButtonLocation kFenderShot(3, 3);
-const ButtonLocation kTrussShot(2, 11);
-const ButtonLocation kHumanPlayerShot(3, 2);
-#else
-const ButtonLocation kFire(3, 9);
-const ButtonLocation kUnload(1, 4);
-const ButtonLocation kReload(1, 2);
-
-const ButtonLocation kRollersOut(3, 8);
-const ButtonLocation kRollersIn(3, 3);
-
-const ButtonLocation kTuck(3, 4);
-const ButtonLocation kIntakePosition(3, 5);
-const ButtonLocation kIntakeOpenPosition(3, 11);
-const ButtonLocation kVerticalTuck(2, 6);
-const JoystickAxis kFlipRobot(3, 3);
-
-const ButtonLocation kLongShot(3, 7);
-const ButtonLocation kCloseShot(3, 6);
-const ButtonLocation kFenderShot(3, 2);
-const ButtonLocation kTrussShot(2, 11);
-const ButtonLocation kHumanPlayerShot(3, 1);
-#endif
-
-const ButtonLocation kUserLeft(2, 7);
-const ButtonLocation kUserRight(2, 10);
-
-const JoystickAxis kAdjustClawGoal(3, 2);
-const JoystickAxis kAdjustClawSeparation(3, 1);
-
-struct ClawGoal {
- double angle;
- double separation;
-};
-
-struct ShotGoal {
- ClawGoal claw;
- double shot_power;
- double velocity_compensation;
- double intake_power;
-};
-
-const double kIntakePower = 4.0;
-// In case we have to quickly adjust it.
-const double kGrabSeparation = 0;
-const double kShootSeparation = 0.11 + kGrabSeparation;
-
-const ClawGoal kTuckGoal = {-2.273474, -0.749484};
-const ClawGoal kVerticalTuckGoal = {0, kGrabSeparation};
-const ClawGoal kIntakeGoal = {-2.24, kGrabSeparation};
-const ClawGoal kIntakeOpenGoal = {-2.0, 1.1};
-
-// TODO(austin): Tune these by hand...
-const ClawGoal kFlippedTuckGoal = {2.733474, -0.75};
-const ClawGoal kFlippedIntakeGoal = {2.0, kGrabSeparation};
-const ClawGoal kFlippedIntakeOpenGoal = {0.95, 1.0};
-
-// 34" between near edge of colored line and rear edge of bumper.
-// Only works running?
-const ShotGoal kLongShotGoal = {
- {-1.08, kShootSeparation}, 145, 0.04, kIntakePower};
-// old 34" {-1.06, kShootSeparation}, 140, 0.04, kIntakePower};
-const ShotGoal kFlippedLongShotGoal = {
- {0.96, kShootSeparation}, 145, 0.09, kIntakePower};
-// old 34" {0.96, kShootSeparation}, 140, 0.09, kIntakePower};
-
-// 78" between near edge of colored line and rear edge of bumper.
-const ShotGoal kCloseShotGoal = {
- {-0.95, kShootSeparation}, 105, 0.2, kIntakePower};
-// 3/4" plunger {-0.90, kShootSeparation}, 105, 0.2, kIntakePower};
-const ShotGoal kFlippedMediumShotGoal = {
- {0.865, kShootSeparation}, 120, 0.2, kIntakePower};
-// 3/4" plunger {0.80, kShootSeparation}, 105, 0.2, kIntakePower};
-
-// Shot from the fender.
-const ShotGoal kFenderShotGoal = {
- {-0.68, kShootSeparation}, 115.0, 0.0, kIntakePower};
-const ShotGoal kFlippedShortShotGoal = {
- {0.63, kShootSeparation}, 115.0, 0.0, kIntakePower};
-
-const ShotGoal kHumanShotGoal = {
- {-0.90, kShootSeparation}, 140, 0.04, kIntakePower};
-const ShotGoal kFlippedHumanShotGoal = {
- {0.90, kShootSeparation}, 140, 0, kIntakePower};
-const ShotGoal kTrussShotGoal = {
- {-0.68, kShootSeparation}, 88.0, 0.4, kIntakePower};
-const ShotGoal kFlippedTrussShotGoal = {
- {0.68, kShootSeparation}, 92.0, 0.4, kIntakePower};
-
-const ShotGoal kFlippedDemoShotGoal = {
- {1.0, kShootSeparation}, 65.0, 0.0, kIntakePower};
-const ShotGoal kDemoShotGoal = {
- {-1.0, kShootSeparation}, 50.0, 0.0, kIntakePower};
-
-const ClawGoal k254PassGoal = {-1.95, kGrabSeparation};
-const ClawGoal kFlipped254PassGoal = {1.96, kGrabSeparation};
-
-// Makes a new ShootAction action.
-::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>
-MakeCatchAction() {
- return ::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>(
- new TypedAction< ::frc971::actions::CatchActionGroup>(
- &::frc971::actions::catch_action));
-}
-
-// A queue which queues Actions and cancels them.
-class ActionQueue {
- public:
- // Queues up an action for sending.
- void QueueAction(::std::unique_ptr<Action> action) {
- if (current_action_) {
- LOG(INFO, "Queueing action, canceling prior\n");
- current_action_->Cancel();
- next_action_ = ::std::move(action);
- } else {
- LOG(INFO, "Queueing action\n");
- current_action_ = ::std::move(action);
- current_action_->Start();
- }
- }
-
- // Cancels the current action, and runs the next one when the current one has
- // finished.
- void CancelCurrentAction() {
- LOG(INFO, "Canceling current action\n");
- if (current_action_) {
- current_action_->Cancel();
- }
- }
-
- // Cancels all running actions.
- void CancelAllActions() {
- LOG(DEBUG, "Cancelling all actions\n");
- if (current_action_) {
- current_action_->Cancel();
- }
- next_action_.reset();
- }
-
- // Runs the next action when the current one is finished running.
- void Tick() {
- if (current_action_) {
- if (!current_action_->Running()) {
- LOG(INFO, "Action is done.\n");
- current_action_ = ::std::move(next_action_);
- if (current_action_) {
- LOG(INFO, "Running next action\n");
- current_action_->Start();
- }
- }
- }
- }
-
- // Returns true if any action is running or could be running.
- // For a one cycle faster response, call Tick before running this.
- bool Running() { return static_cast<bool>(current_action_); }
-
- private:
- ::std::unique_ptr<Action> current_action_;
- ::std::unique_ptr<Action> next_action_;
-};
-
+const ButtonLocation kFrontRollersIn(3, 5);
+const ButtonLocation kBackRollersIn(3, 3);
+const ButtonLocation kFrontRollersOut(3, 12);
+const ButtonLocation kBackRollersOut(3, 8);
+const ButtonLocation kHumanPlayer(3, 11);
class Reader : public ::aos::input::JoystickInput {
public:
Reader()
- : is_high_gear_(false),
- shot_power_(80.0),
- goal_angle_(0.0),
- separation_angle_(kGrabSeparation),
- velocity_compensation_(0.0),
- intake_power_(0.0),
- was_running_(false) {}
+ : is_high_gear_(false) {}
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
if (data.GetControlBit(ControlBit::kAutonomous)) {
@@ -245,24 +57,13 @@
.run_auto(false)
.Send();
} else if (!data.GetControlBit(ControlBit::kEnabled)) {
- {
- auto goal = drivetrain.goal.MakeMessage();
- goal->Zero();
- goal->control_loop_driving = false;
- goal->left_goal = goal->right_goal = 0;
- goal->left_velocity_goal = goal->right_velocity_goal = 0;
- if (!goal.Send()) {
- LOG(WARNING, "sending 0 drivetrain goal failed\n");
- }
- }
- {
- // TODO(brians): Make sure this doesn't make it unbrake and not move
- // or something weird.
- auto goal = control_loops::shooter_queue_group.goal.MakeMessage();
- goal->Zero();
- if (!goal.Send()) {
- LOG(WARNING, "sending 0 shooter goal failed\n");
- }
+ auto goal = drivetrain.goal.MakeMessage();
+ goal->Zero();
+ goal->control_loop_driving = false;
+ goal->left_goal = goal->right_goal = 0;
+ goal->left_velocity_goal = goal->right_velocity_goal = 0;
+ if (!goal.Send()) {
+ LOG(WARNING, "sending 0 drivetrain goal failed\n");
}
}
} else {
@@ -279,7 +80,6 @@
const double kThrottleGain = 1.0 / 2.5;
if (false && (data.IsPressed(kDriveControlLoopEnable1) ||
data.IsPressed(kDriveControlLoopEnable2))) {
- // TODO(austin): Static sucks!
static double distance = 0.0;
static double angle = 0.0;
static double filtered_goal_distance = 0.0;
@@ -336,278 +136,38 @@
}
}
- void SetGoal(ClawGoal goal) {
- goal_angle_ = goal.angle;
- separation_angle_ = goal.separation;
- moving_for_shot_ = false;
- velocity_compensation_ = 0.0;
- intake_power_ = 0.0;
- }
-
- void SetGoal(ShotGoal goal) {
- goal_angle_ = goal.claw.angle;
- shot_separation_angle_ = goal.claw.separation;
- separation_angle_ = kGrabSeparation;
- moving_for_shot_ = true;
- shot_power_ = goal.shot_power;
- velocity_compensation_ = goal.velocity_compensation;
- intake_power_ = goal.intake_power;
- }
-
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
HandleDrivetrain(data);
- if (!data.GetControlBit(ControlBit::kEnabled)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
+
+ // Rollers.
+ auto rollers_goal = control_loops::rollers.goal.MakeMessage();
+ if (data.IsPressed(kFrontRollersIn)) {
+ rollers_goal->intake = 1;
+ } else if (data.IsPressed(kFrontRollersOut)) {
+ rollers_goal->low_spit = 1;
+ } else if (data.IsPressed(kBackRollersIn)) {
+ rollers_goal->intake = -1;
+ } else if (data.IsPressed(kBackRollersOut)) {
+ rollers_goal->low_spit = -1;
+ } else if (data.IsPressed(kHumanPlayer)) {
+ rollers_goal->human_player = true;
}
- if (data.IsPressed(kRollersIn) || data.IsPressed(kRollersOut)) {
- intake_power_ = 0.0;
- separation_angle_ = kGrabSeparation;
- moving_for_shot_ = false;
+ if (!rollers_goal.Send()) {
+ LOG(WARNING, "Sending rollers values failed.\n");
}
-
- static const double kAdjustClawGoalDeadband = 0.08;
- double claw_goal_adjust = data.GetAxis(kAdjustClawGoal);
- if (OLD_DS || ::std::abs(claw_goal_adjust) < kAdjustClawGoalDeadband) {
- claw_goal_adjust = 0;
- } else {
- claw_goal_adjust = (claw_goal_adjust -
- ((claw_goal_adjust < 0) ? -kAdjustClawGoalDeadband
- : kAdjustClawGoalDeadband)) *
- 0.035;
- }
- double claw_separation_adjust = data.GetAxis(kAdjustClawSeparation);
- if (OLD_DS ||
- ::std::abs(claw_separation_adjust) < kAdjustClawGoalDeadband) {
- claw_separation_adjust = 0;
- } else {
- claw_separation_adjust =
- (claw_separation_adjust -
- ((claw_separation_adjust < 0) ? -kAdjustClawGoalDeadband
- : kAdjustClawGoalDeadband)) *
- -0.035;
- }
-
-#if OLD_DS
- if (data.IsPressed(kFenderShot)) {
-#else
- if (data.GetAxis(kFlipRobot) > 0.9) {
-#endif
- claw_goal_adjust += claw_separation_adjust;
- claw_goal_adjust *= -1;
-
- if (data.IsPressed(kIntakeOpenPosition)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedIntakeOpenGoal);
- } else if (data.IsPressed(kIntakePosition)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedIntakeGoal);
- } else if (data.IsPressed(kVerticalTuck)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kVerticalTuckGoal);
- } else if (data.IsPressed(kTuck)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedTuckGoal);
- } else if (data.PosEdge(kLongShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedLongShotGoal);
- } else if (data.PosEdge(kCloseShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedMediumShotGoal);
- } else if (data.PosEdge(kFenderShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedShortShotGoal);
- } else if (data.PosEdge(kHumanPlayerShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedHumanShotGoal);
- } else if (data.PosEdge(kUserLeft)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlipped254PassGoal);
- } else if (data.PosEdge(kUserRight)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedDemoShotGoal);
- } else if (data.PosEdge(kTrussShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFlippedTrussShotGoal);
- }
- } else {
- if (data.IsPressed(kIntakeOpenPosition)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kIntakeOpenGoal);
- } else if (data.IsPressed(kIntakePosition)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kIntakeGoal);
- } else if (data.IsPressed(kVerticalTuck)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kVerticalTuckGoal);
- } else if (data.IsPressed(kTuck)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kTuckGoal);
- } else if (data.PosEdge(kLongShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kLongShotGoal);
- } else if (data.PosEdge(kCloseShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kCloseShotGoal);
- } else if (data.PosEdge(kFenderShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kFenderShotGoal);
- } else if (data.PosEdge(kHumanPlayerShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kHumanShotGoal);
- } else if (data.PosEdge(kUserLeft)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(k254PassGoal);
- } else if (data.PosEdge(kUserRight)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kDemoShotGoal);
- } else if (data.PosEdge(kTrussShot)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- SetGoal(kTrussShotGoal);
- }
- }
-
- /*
- if (data.PosEdge(kCatch)) {
- auto catch_action = MakeCatchAction();
- catch_action->GetGoal()->catch_angle = goal_angle_;
- action_queue_.QueueAction(::std::move(catch_action));
- }
- */
-
- if (data.PosEdge(kFire)) {
- action_queue_.QueueAction(actions::MakeShootAction());
- } else if (data.NegEdge(kFire)) {
- action_queue_.CancelCurrentAction();
- }
-
- action_queue_.Tick();
- if (data.IsPressed(kUnload) || data.IsPressed(kReload)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- intake_power_ = 0.0;
- velocity_compensation_ = 0.0;
- }
-
- // Send out the claw and shooter goals if no actions are running.
- if (!action_queue_.Running()) {
- goal_angle_ += claw_goal_adjust;
- separation_angle_ += claw_separation_adjust;
-
- // If the action just ended, turn the intake off and stop velocity
- // compensating.
- if (was_running_) {
- intake_power_ = 0.0;
- velocity_compensation_ = 0.0;
- }
-
- control_loops::drivetrain.status.FetchLatest();
- double goal_angle = goal_angle_;
- if (control_loops::drivetrain.status.get()) {
- goal_angle +=
- SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed);
- } else {
- LOG_INTERVAL(no_drivetrain_status_);
- }
-
- if (moving_for_shot_) {
- auto &claw_status = control_loops::claw_queue_group.status;
- claw_status.FetchLatest();
- if (claw_status.get()) {
- if (::std::abs(claw_status->bottom - goal_angle) < 0.2) {
- moving_for_shot_ = false;
- separation_angle_ = shot_separation_angle_;
- }
- }
- }
-
- double separation_angle = separation_angle_;
-
- if (data.IsPressed(kCatch)) {
- const double kCatchSeparation = 1.0;
- goal_angle -= kCatchSeparation / 2.0;
- separation_angle = kCatchSeparation;
- }
-
- bool intaking =
- data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition) ||
- data.IsPressed(kIntakeOpenPosition) || data.IsPressed(kCatch);
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(goal_angle)
- .separation_angle(separation_angle)
- .intake(intaking ? 12.0
- : (data.IsPressed(kRollersOut) ? -12.0
- : intake_power_))
- .centering(intaking ? 12.0 : 0.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
-
- if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
- .shot_power(shot_power_)
- .shot_requested(data.IsPressed(kFire))
- .unload_requested(data.IsPressed(kUnload))
- .load_requested(data.IsPressed(kReload))
- .Send()) {
- LOG(WARNING, "sending shooter goal failed\n");
- }
- }
- was_running_ = action_queue_.Running();
- }
-
- double SpeedToAngleOffset(double speed) {
- const frc971::constants::Values &values = frc971::constants::GetValues();
- // scale speed to a [0.0-1.0] on something close to the max
- // TODO(austin): Change the scale factor for different shots.
- return (speed / values.drivetrain_max_speed) * velocity_compensation_;
}
private:
bool is_high_gear_;
- double shot_power_;
- double goal_angle_;
- double separation_angle_, shot_separation_angle_;
- double velocity_compensation_;
- double intake_power_;
- bool was_running_;
- bool moving_for_shot_ = false;
-
- ActionQueue action_queue_;
-
- ::aos::util::SimpleLogInterval no_drivetrain_status_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
- "no drivetrain status");
};
} // namespace joysticks
} // namespace input
-} // namespace frc971
+} // namespace bot3
int main() {
::aos::Init();
- ::frc971::input::joysticks::Reader reader;
+ ::bot3::input::joysticks::Reader reader;
reader.Run();
::aos::Cleanup();
}