Merge "Fix R63 label being on top of pads"
diff --git a/aos/common/commonmath.h b/aos/common/commonmath.h
index f9337b2..acbcb1c 100644
--- a/aos/common/commonmath.h
+++ b/aos/common/commonmath.h
@@ -1,6 +1,8 @@
#ifndef AOS_COMMON_MATH_H_
#define AOS_COMMON_MATH_H_
+#include <cmath>
+
namespace aos {
// Clips a value so that it is in [min, max]
@@ -22,6 +24,20 @@
}
}
+// Adds deadband to provided value. deadband is the region close to the origin
+// to add the deadband to, and max is the maximum input value used to re-scale
+// the output after adding the deadband.
+static inline double Deadband(double value, const double deadband,
+ const double max) {
+ if (::std::abs(value) < deadband) {
+ value = 0.0;
+ } else if (value > 0.0) {
+ value = (value - deadband) / (max - deadband);
+ } else {
+ value = (value + deadband) / (max - deadband);
+ }
+ return value;
+}
} // namespace aos
#endif // AOS_COMMON_MATH_H_
diff --git a/aos/common/logging/implementations_test.cc b/aos/common/logging/implementations_test.cc
index aade0d1..9663fd2 100644
--- a/aos/common/logging/implementations_test.cc
+++ b/aos/common/logging/implementations_test.cc
@@ -211,10 +211,10 @@
// Make sure rounding works correctly.
ASSERT_EQ(18, snprintf(buffer, sizeof(buffer), AOS_TIME_FORMAT,
AOS_TIME_ARGS(2, 999)));
- EXPECT_EQ("0000000002.000001s", ::std::string(buffer));
+ EXPECT_EQ("0000000002.000000s", ::std::string(buffer));
ASSERT_EQ(18, snprintf(buffer, sizeof(buffer), AOS_TIME_FORMAT,
AOS_TIME_ARGS(2, 500)));
- EXPECT_EQ("0000000002.000001s", ::std::string(buffer));
+ EXPECT_EQ("0000000002.000000s", ::std::string(buffer));
ASSERT_EQ(18, snprintf(buffer, sizeof(buffer), AOS_TIME_FORMAT,
AOS_TIME_ARGS(2, 499)));
EXPECT_EQ("0000000002.000000s", ::std::string(buffer));
@@ -223,6 +223,11 @@
ASSERT_EQ(18, snprintf(buffer, sizeof(buffer), AOS_TIME_FORMAT,
AOS_TIME_ARGS(1, 995000000)));
EXPECT_EQ("0000000001.995000s", ::std::string(buffer));
+
+ // This used to result in "0000000001.099500s".
+ ASSERT_EQ(18, snprintf(buffer, sizeof(buffer), AOS_TIME_FORMAT,
+ AOS_TIME_ARGS(1, 999999999)));
+ EXPECT_EQ("0000000001.999999s", ::std::string(buffer));
}
TEST(LoggingPrintFormatTest, Base) {
diff --git a/aos/common/logging/printf_formats.h b/aos/common/logging/printf_formats.h
index b098865..60960a4 100644
--- a/aos/common/logging/printf_formats.h
+++ b/aos/common/logging/printf_formats.h
@@ -11,9 +11,7 @@
#define AOS_TIME_FORMAT \
"%010" PRId32 ".%0" STRINGIFY(AOS_TIME_NSECONDS_DIGITS) PRId32 "s"
-#define AOS_TIME_ARGS(sec, nsec) \
- sec, (nsec + (AOS_TIME_NSECONDS_DENOMINATOR / 2)) / \
- AOS_TIME_NSECONDS_DENOMINATOR
+#define AOS_TIME_ARGS(sec, nsec) sec, (nsec / AOS_TIME_NSECONDS_DENOMINATOR)
#define AOS_LOGGING_BASE_FORMAT \
"%.*s(%" PRId32 ")(%05" PRIu16 "): %-7s at " AOS_TIME_FORMAT ": "
diff --git a/aos/input/BUILD b/aos/input/BUILD
index 30cb053..d8fc2c8 100644
--- a/aos/input/BUILD
+++ b/aos/input/BUILD
@@ -16,3 +16,22 @@
'//aos/common/logging:queue_logging',
],
)
+
+cc_library(
+ name = 'drivetrain_input',
+ srcs = [
+ 'drivetrain_input.cc'
+ ],
+ hdrs = [
+ 'drivetrain_input.h',
+ ],
+ deps = [
+ '//aos/common/input:driver_station_data',
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/messages:robot_state',
+ '//aos/common/network:socket',
+ '//aos/common:math',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ ],
+)
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
new file mode 100644
index 0000000..a0d95cf
--- /dev/null
+++ b/aos/input/drivetrain_input.cc
@@ -0,0 +1,188 @@
+#include "aos/input/drivetrain_input.h"
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <cmath>
+
+#include "aos/common/commonmath.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::ControlBit;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::POVLocation;
+
+namespace aos {
+namespace input {
+
+void DrivetrainInputReader::HandleDrivetrain(
+ const ::aos::input::driver_station::Data &data) {
+ bool is_control_loop_driving = false;
+
+ const auto wheel_and_throttle = GetWheelAndThrottle(data);
+ const double wheel = wheel_and_throttle.wheel;
+ const double throttle = wheel_and_throttle.throttle;
+
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ robot_velocity_ = drivetrain_queue.status->robot_speed;
+ }
+
+ if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
+ if (drivetrain_queue.status.get()) {
+ left_goal_ = drivetrain_queue.status->estimated_left_position;
+ right_goal_ = drivetrain_queue.status->estimated_right_position;
+ }
+ }
+ const double current_left_goal =
+ left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
+ const double current_right_goal =
+ right_goal_ + wheel * wheel_multiplier_ + throttle * 0.3;
+ if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
+ is_control_loop_driving = true;
+ }
+ auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
+ new_drivetrain_goal->steering = wheel;
+ new_drivetrain_goal->throttle = throttle;
+ new_drivetrain_goal->quickturn = data.IsPressed(kQuickTurn);
+ new_drivetrain_goal->control_loop_driving = is_control_loop_driving;
+ new_drivetrain_goal->left_goal = current_left_goal;
+ new_drivetrain_goal->right_goal = current_right_goal;
+ new_drivetrain_goal->left_velocity_goal = 0;
+ new_drivetrain_goal->right_velocity_goal = 0;
+
+ new_drivetrain_goal->linear.max_velocity = 3.0;
+ new_drivetrain_goal->linear.max_acceleration = 20.0;
+
+ if (!new_drivetrain_goal.Send()) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+}
+
+DrivetrainInputReader::WheelAndThrottle
+SteeringWheelDrivetrainInputReader::GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) {
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+}
+
+DrivetrainInputReader::WheelAndThrottle
+PistolDrivetrainInputReader::GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) {
+ const double unscaled_wheel = data.GetAxis(kSteeringWheel);
+ double wheel;
+ if (unscaled_wheel < 0.0) {
+ wheel = unscaled_wheel / 0.484375;
+ } else {
+ wheel = unscaled_wheel / 0.385827;
+ }
+
+ const double unscaled_throttle = -data.GetAxis(kDriveThrottle);
+ double unmodified_throttle;
+ if (unscaled_throttle < 0.0) {
+ unmodified_throttle = unscaled_throttle / 0.086614;
+ } else {
+ unmodified_throttle = unscaled_throttle / 0.265625;
+ }
+ unmodified_throttle = aos::Deadband(unmodified_throttle, 0.1, 1.0);
+
+ // Apply a sin function that's scaled to make it feel better.
+ constexpr double throttle_range = M_PI_2 * 0.5;
+
+ double throttle = ::std::sin(throttle_range * unmodified_throttle) /
+ ::std::sin(throttle_range);
+ throttle = ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
+ throttle = 2.0 * unmodified_throttle - throttle;
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+}
+
+DrivetrainInputReader::WheelAndThrottle
+XboxDrivetrainInputReader::GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) {
+ // xbox
+ constexpr double kWheelDeadband = 0.05;
+ constexpr double kThrottleDeadband = 0.05;
+ const double wheel =
+ aos::Deadband(-data.GetAxis(kSteeringWheel), kWheelDeadband, 1.0);
+
+ const double unmodified_throttle =
+ aos::Deadband(-data.GetAxis(kDriveThrottle), kThrottleDeadband, 1.0);
+
+ // Apply a sin function that's scaled to make it feel better.
+ constexpr double throttle_range = M_PI_2 * 0.9;
+
+ double throttle = ::std::sin(throttle_range * unmodified_throttle) /
+ ::std::sin(throttle_range);
+ throttle = ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
+ throttle = 2.0 * unmodified_throttle - throttle;
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+}
+
+std::unique_ptr<SteeringWheelDrivetrainInputReader>
+SteeringWheelDrivetrainInputReader::Make() {
+ const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+ const ButtonLocation kQuickTurn(1, 5);
+ const ButtonLocation kTurn1(1, 7);
+ const ButtonLocation kTurn2(1, 11);
+ std::unique_ptr<SteeringWheelDrivetrainInputReader> result(
+ new SteeringWheelDrivetrainInputReader(kSteeringWheel, kDriveThrottle,
+ kQuickTurn, kTurn1, kTurn2));
+ return result;
+}
+
+std::unique_ptr<PistolDrivetrainInputReader>
+PistolDrivetrainInputReader::Make() {
+ // Pistol Grip controller
+ const JoystickAxis kSteeringWheel(1, 2), kDriveThrottle(1, 1);
+ const ButtonLocation kQuickTurn(1, 7);
+ const ButtonLocation kTurn1(1, 8);
+
+ // Nop
+ const ButtonLocation kTurn2(1, 9);
+ std::unique_ptr<PistolDrivetrainInputReader> result(
+ new PistolDrivetrainInputReader(kSteeringWheel, kDriveThrottle,
+ kQuickTurn, kTurn1, kTurn2));
+ result->set_wheel_multiplier(0.2);
+ return result;
+}
+
+std::unique_ptr<XboxDrivetrainInputReader> XboxDrivetrainInputReader::Make() {
+ // xbox
+ const JoystickAxis kSteeringWheel(1, 5), kDriveThrottle(1, 2);
+ const ButtonLocation kQuickTurn(1, 5);
+
+ // Nop
+ const ButtonLocation kTurn1(1, 1);
+ const ButtonLocation kTurn2(1, 2);
+
+ std::unique_ptr<XboxDrivetrainInputReader> result(
+ new XboxDrivetrainInputReader(kSteeringWheel, kDriveThrottle, kQuickTurn,
+ kTurn1, kTurn2));
+ return result;
+}
+::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
+ InputType type) {
+ std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader;
+
+ using InputType = DrivetrainInputReader::InputType;
+ switch (type) {
+ case InputType::kSteeringWheel:
+ drivetrain_input_reader = SteeringWheelDrivetrainInputReader::Make();
+ break;
+ case InputType::kPistol:
+ drivetrain_input_reader = PistolDrivetrainInputReader::Make();
+ break;
+ case InputType::kXbox:
+ drivetrain_input_reader = XboxDrivetrainInputReader::Make();
+ break;
+ }
+ return drivetrain_input_reader;
+}
+
+} // namespace input
+} // namespace aos
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
new file mode 100644
index 0000000..4b13ac6
--- /dev/null
+++ b/aos/input/drivetrain_input.h
@@ -0,0 +1,144 @@
+#ifndef AOS_INPUT_DRIVETRAIN_INPUT_H_
+#define AOS_INPUT_DRIVETRAIN_INPUT_H_
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <cmath>
+#include <memory>
+
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+namespace aos {
+namespace input {
+
+// We have a couple different joystick configurations used to drive our skid
+// steer robots. These configurations don't change very often, and there are a
+// small, discrete, set of them. The interface to the drivetrain is the same
+// across all of our drivetrains, so we can abstract that away from our users.
+//
+// We want to be able to re-use that code across years, and switch joystick
+// types quickly and efficiently.
+//
+// DrivetrainInputReader is the abstract base class which provides a consistent
+// API to control drivetrains.
+//
+// To construct the appropriate DrivetrainInputReader, call
+// DrivetrainInputReader::Make with the input type enum.
+// To use it, call HandleDrivetrain(data) every time a joystick packet is
+// received.
+//
+// Base class for the interface to the drivetrain implemented by multiple
+// joystick types.
+class DrivetrainInputReader {
+ public:
+ // Inputs driver station button and joystick locations
+ DrivetrainInputReader(driver_station::JoystickAxis kSteeringWheel,
+ driver_station::JoystickAxis kDriveThrottle,
+ driver_station::ButtonLocation kQuickTurn,
+ driver_station::ButtonLocation kTurn1,
+ driver_station::ButtonLocation kTurn2)
+ : kSteeringWheel(kSteeringWheel),
+ kDriveThrottle(kDriveThrottle),
+ kQuickTurn(kQuickTurn),
+ kTurn1(kTurn1),
+ kTurn2(kTurn2) {}
+
+ virtual ~DrivetrainInputReader() = default;
+
+ // List of driver joystick types.
+ enum class InputType {
+ kSteeringWheel,
+ kPistol,
+ kXbox,
+ };
+
+ // Constructs the appropriate DrivetrainInputReader.
+ static std::unique_ptr<DrivetrainInputReader> Make(InputType type);
+
+ // Processes new joystick data and publishes drivetrain goal messages.
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data);
+
+ // Sets the scalar for the steering wheel for closed loop mode converting
+ // steering ratio to meters displacement on the two wheels.
+ void set_wheel_multiplier(double wheel_multiplier) {
+ wheel_multiplier_ = wheel_multiplier;
+ }
+
+ // Returns the current robot velocity in m/s.
+ double robot_velocity() const { return robot_velocity_; }
+
+ protected:
+ const driver_station::JoystickAxis kSteeringWheel;
+ const driver_station::JoystickAxis kDriveThrottle;
+ const driver_station::ButtonLocation kQuickTurn;
+ const driver_station::ButtonLocation kTurn1;
+ const driver_station::ButtonLocation kTurn2;
+
+ // Structure containing the (potentially adjusted) steering and throttle
+ // values from the joysticks.
+ struct WheelAndThrottle {
+ double wheel;
+ double throttle;
+ };
+
+ private:
+ // Computes the steering and throttle from the provided driverstation data.
+ virtual WheelAndThrottle GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) = 0;
+
+ double robot_velocity_ = 0.0;
+ // Goals to send to the drivetrain in closed loop mode.
+ double left_goal_ = 0.0;
+ double right_goal_ = 0.0;
+ // The scale for the joysticks for closed loop mode converting
+ // joysticks to meters displacement on the two wheels.
+ double wheel_multiplier_ = 0.5;
+};
+
+// Implements DrivetrainInputReader for the original steering wheel.
+class SteeringWheelDrivetrainInputReader : public DrivetrainInputReader {
+ public:
+ using DrivetrainInputReader::DrivetrainInputReader;
+
+ // Creates a DrivetrainInputReader with the corresponding joystick ports and
+ // axis for the big steering wheel and throttle stick.
+ static std::unique_ptr<SteeringWheelDrivetrainInputReader> Make();
+
+ private:
+ WheelAndThrottle GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) override;
+};
+
+class PistolDrivetrainInputReader : public DrivetrainInputReader {
+ public:
+ using DrivetrainInputReader::DrivetrainInputReader;
+
+ // Creates a DrivetrainInputReader with the corresponding joystick ports and
+ // axis for the (cheap) pistol grip controller.
+ static std::unique_ptr<PistolDrivetrainInputReader> Make();
+
+ private:
+ WheelAndThrottle GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) override;
+};
+
+class XboxDrivetrainInputReader : public DrivetrainInputReader {
+ public:
+ using DrivetrainInputReader::DrivetrainInputReader;
+
+ // Creates a DrivetrainInputReader with the corresponding joystick ports and
+ // axis for the Xbox controller.
+ static std::unique_ptr<XboxDrivetrainInputReader> Make();
+
+ private:
+ WheelAndThrottle GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) override;
+};
+
+} // namespace input
+} // namespace aos
+
+#endif // AOS_INPUT_DRIVETRAIN_INPUT_H_
diff --git a/setup_roborio.sh b/setup_roborio.sh
new file mode 100755
index 0000000..d7a4144
--- /dev/null
+++ b/setup_roborio.sh
@@ -0,0 +1,32 @@
+#!/bin/bash
+cd $(dirname $0)
+pwd
+
+set -e
+
+if [ $# != 1 ];
+then
+ echo "Usage: setup_robot.sh 971"
+ exit 1
+fi
+
+readonly ROBOT_HOSTNAME="$1"
+
+bazel build -c opt @arm_frc_linux_gnueabi_repo//...
+
+ssh-copy-id "admin@${ROBOT_HOSTNAME}"
+
+if $(ssh "admin@${ROBOT_HOSTNAME}" "cat /etc/profile" | grep -Fq "alias l");
+then
+ echo "Already has l alias"
+else
+ echo "Adding l alias"
+ ssh "admin@${ROBOT_HOSTNAME}" 'echo "alias l=\"ls -la\"" >> /etc/profile'
+fi
+
+ssh "admin@${ROBOT_HOSTNAME}" 'PATH="${PATH}":/usr/local/natinst/bin/ /usr/local/frc/bin/frcKillRobot.sh -r -t'
+
+echo "Deploying robotCommand startup script"
+scp aos/config/robotCommand "admin@${ROBOT_HOSTNAME}:/home/lvuser/"
+
+scp bazel-971-Robot-Code/external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi/lib/libstdc++.so.6.0.20 "admin@${ROBOT_HOSTNAME}:/usr/lib/"
diff --git a/y2014/control_loops/claw/BUILD b/y2014/control_loops/claw/BUILD
index 5602bbc..45a77b0 100644
--- a/y2014/control_loops/claw/BUILD
+++ b/y2014/control_loops/claw/BUILD
@@ -69,11 +69,12 @@
'claw_lib_test.cc',
],
deps = [
- '//aos/testing:googletest',
- ':claw_queue',
':claw_lib',
- '//frc971/control_loops:state_feedback_loop',
+ ':claw_queue',
'//aos/common/controls:control_loop_test',
+ '//aos/testing:googletest',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:team_number_test_environment',
],
)
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index 8444cea..8a7e9c3 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -4,7 +4,7 @@
#include <memory>
#include "aos/common/controls/control_loop_test.h"
-#include "aos/common/network/team_number.h"
+#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2014/constants.h"
#include "y2014/control_loops/claw/claw.h"
@@ -28,15 +28,6 @@
CLAW_COUNT
} ClawType;
-class TeamNumberEnvironment : public ::testing::Environment {
- public:
- // Override this to define how to set up the environment.
- virtual void SetUp() { aos::network::OverrideTeamNumber(1); }
-};
-
-::testing::Environment* const team_number_env =
- ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
-
// Class which simulates the wrist and sends out queue messages containing the
// position.
class ClawMotorSimulation {
diff --git a/y2014/control_loops/shooter/BUILD b/y2014/control_loops/shooter/BUILD
index c10baae..8195060 100644
--- a/y2014/control_loops/shooter/BUILD
+++ b/y2014/control_loops/shooter/BUILD
@@ -68,11 +68,12 @@
'shooter_lib_test.cc',
],
deps = [
- '//aos/testing:googletest',
- ':shooter_queue',
':shooter_lib',
+ ':shooter_queue',
'//aos/common/controls:control_loop_test',
+ '//aos/testing:googletest',
'//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:team_number_test_environment',
],
)
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index df3f5fd..851e617 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -3,13 +3,14 @@
#include <chrono>
#include <memory>
-#include "gtest/gtest.h"
-#include "aos/common/network/team_number.h"
#include "aos/common/controls/control_loop_test.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/control_loops/shooter/shooter.h"
-#include "y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h"
+#include "aos/common/network/team_number.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
#include "y2014/constants.h"
+#include "y2014/control_loops/shooter/shooter.h"
+#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h"
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
@@ -20,18 +21,7 @@
using ::y2014::control_loops::shooter::kMaxExtension;
using ::y2014::control_loops::shooter::MakeRawShooterPlant;
-
-static const int kTestTeam = 1;
-
-class TeamNumberEnvironment : public ::testing::Environment {
- public:
- // Override this to define how to set up the environment.
- virtual void SetUp() { aos::network::OverrideTeamNumber(kTestTeam); }
-};
-
-::testing::Environment *const team_number_env =
- ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
-
+using ::frc971::control_loops::testing::kTeamNumber;
// Class which simulates the shooter and sends out queue messages containing the
// position.
@@ -463,9 +453,8 @@
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SimulateTimestep(true);
- EXPECT_LT(
- shooter_motor_plant_.GetAbsolutePosition(),
- constants::GetValuesForTeam(kTestTeam).shooter.upper_limit);
+ EXPECT_LT(shooter_motor_plant_.GetAbsolutePosition(),
+ constants::GetValuesForTeam(kTeamNumber).shooter.upper_limit);
}
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -725,12 +714,12 @@
::testing::Bool(), ::testing::Bool(), ::testing::Bool(),
::testing::Values(
0.05,
- constants::GetValuesForTeam(kTestTeam).shooter.upper_limit - 0.05,
- HallEffectMiddle(constants::GetValuesForTeam(kTestTeam)
+ constants::GetValuesForTeam(kTeamNumber).shooter.upper_limit - 0.05,
+ HallEffectMiddle(constants::GetValuesForTeam(kTeamNumber)
.shooter.pusher_proximal),
- HallEffectMiddle(constants::GetValuesForTeam(kTestTeam)
- .shooter.pusher_distal),
- constants::GetValuesForTeam(kTestTeam)
+ HallEffectMiddle(
+ constants::GetValuesForTeam(kTeamNumber).shooter.pusher_distal),
+ constants::GetValuesForTeam(kTeamNumber)
.shooter.latch_max_safe_position -
0.001)));
diff --git a/y2017/BUILD b/y2017/BUILD
index 4bf45fa..763e331 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -36,6 +36,7 @@
'//aos/common/util:log_interval',
'//aos/common:time',
'//aos/input:joystick_input',
+ '//aos/input:drivetrain_input',
'//aos/linux_code:init',
'//frc971/autonomous:auto_queue',
'//frc971/control_loops/drivetrain:drivetrain_queue',
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index c0113c3..e0f66ba 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -8,6 +8,7 @@
#include "aos/common/logging/logging.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
+#include "aos/input/drivetrain_input.h"
#include "aos/input/joystick_input.h"
#include "aos/linux_code/init.h"
#include "frc971/autonomous/auto.q.h"
@@ -23,46 +24,12 @@
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::POVLocation;
+using ::aos::input::DrivetrainInputReader;
namespace y2017 {
namespace input {
namespace joysticks {
-// Keep the other versions around so we can switch quickly.
-//#define STEERINGWHEEL
-#define PISTOL
-//#define XBOX
-
-#ifdef STEERINGWHEEL
-const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kTurn1(1, 7);
-const ButtonLocation kTurn2(1, 11);
-
-#endif
-
-#ifdef PISTOL
-// Pistol Grip controller
-const JoystickAxis kSteeringWheel(1, 2), kDriveThrottle(1, 1);
-const ButtonLocation kQuickTurn(1, 7);
-const ButtonLocation kTurn1(1, 8);
-
-// Nop
-const ButtonLocation kTurn2(1, 9);
-#endif
-
-#ifdef XBOX
-// xbox
-const JoystickAxis kSteeringWheel(1, 5), kDriveThrottle(1, 2);
-const ButtonLocation kQuickTurn(1, 5);
-
-// Nop
-const ButtonLocation kTurn1(1, 1);
-const ButtonLocation kTurn2(1, 2);
-
-#endif
-
-
const ButtonLocation kGearSlotBack(2, 11);
const ButtonLocation kIntakeDown(3, 9);
@@ -81,9 +48,14 @@
const ButtonLocation kExtra2(3, 10);
const ButtonLocation kHang(3, 2);
+std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
+
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() {}
+ Reader() {
+ drivetrain_input_reader_ = DrivetrainInputReader::Make(
+ DrivetrainInputReader::InputType::kSteeringWheel);
+ }
enum class ShotDistance { VISION_SHOT, MIDDLE_SHOT, FAR_SHOT };
@@ -112,116 +84,9 @@
}
int intake_accumulator_ = 0;
- double Deadband(double value, const double deadband) {
- if (::std::abs(value) < deadband) {
- value = 0.0;
- } else if (value > 0.0) {
- value = (value - deadband) / (1.0 - deadband);
- } else {
- value = (value + deadband) / (1.0 - deadband);
- }
- return value;
- }
-
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- bool is_control_loop_driving = false;
-
-#ifdef STEERINGWHEEL
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
-#endif
-
-#ifdef XBOX
- // xbox
- constexpr double kWheelDeadband = 0.05;
- constexpr double kThrottleDeadband = 0.05;
- const double wheel =
- Deadband(-data.GetAxis(kSteeringWheel), kWheelDeadband);
-
- const double unmodified_throttle =
- Deadband(-data.GetAxis(kDriveThrottle), kThrottleDeadband);
-
- // Apply a sin function that's scaled to make it feel better.
- constexpr double throttle_range = M_PI_2 * 0.9;
-
- double throttle = ::std::sin(throttle_range * unmodified_throttle) /
- ::std::sin(throttle_range);
- throttle =
- ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
- throttle = 2.0 * unmodified_throttle - throttle;
-#endif
-
-#ifdef PISTOL
- const double unscaled_wheel = data.GetAxis(kSteeringWheel);
- double wheel;
- if (unscaled_wheel < 0.0) {
- wheel = unscaled_wheel / 0.484375;
- } else {
- wheel = unscaled_wheel / 0.385827;
- }
-
- const double unscaled_throttle = -data.GetAxis(kDriveThrottle);
- double unmodified_throttle;
- if (unscaled_throttle < 0.0) {
- unmodified_throttle = unscaled_throttle / 0.086614;
- } else {
- unmodified_throttle = unscaled_throttle / 0.265625;
- }
- unmodified_throttle = Deadband(unmodified_throttle, 0.1);
-
- // Apply a sin function that's scaled to make it feel better.
- constexpr double throttle_range = M_PI_2 * 0.5;
-
- double throttle = ::std::sin(throttle_range * unmodified_throttle) /
- ::std::sin(throttle_range);
- throttle =
- ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
- throttle = 2.0 * unmodified_throttle - throttle;
-#endif
-
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
- robot_velocity_ = drivetrain_queue.status->robot_speed;
- }
-
- if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2) ||
- data.PosEdge(kGearSlotBack)) {
- if (drivetrain_queue.status.get()) {
- left_goal_ = drivetrain_queue.status->estimated_left_position;
- right_goal_ = drivetrain_queue.status->estimated_right_position;
- }
- }
-#ifdef PISTOL
- double current_left_goal = left_goal_ - wheel * 0.20 + throttle * 0.3;
- double current_right_goal = right_goal_ + wheel * 0.20 + throttle * 0.3;
-#else
- double current_left_goal = left_goal_ - wheel * 0.5 + throttle * 0.3;
- double current_right_goal = right_goal_ + wheel * 0.5 + throttle * 0.3;
-#endif
- if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
- is_control_loop_driving = true;
- }
- if (data.IsPressed(kGearSlotBack)) {
- is_control_loop_driving = true;
- current_left_goal = left_goal_ - 0.03;
- current_right_goal = right_goal_ - 0.03;
- }
- auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
- new_drivetrain_goal->steering = wheel;
- new_drivetrain_goal->throttle = throttle;
- new_drivetrain_goal->quickturn = data.IsPressed(kQuickTurn);
- new_drivetrain_goal->control_loop_driving = is_control_loop_driving;
- new_drivetrain_goal->left_goal = current_left_goal;
- new_drivetrain_goal->right_goal = current_right_goal;
- new_drivetrain_goal->left_velocity_goal = 0;
- new_drivetrain_goal->right_velocity_goal = 0;
-
- new_drivetrain_goal->linear.max_velocity = 3.0;
- new_drivetrain_goal->linear.max_acceleration = 20.0;
-
- if (!new_drivetrain_goal.Send()) {
- LOG(WARNING, "sending stick values failed\n");
- }
+ drivetrain_input_reader_->HandleDrivetrain(data);
+ robot_velocity_ = drivetrain_input_reader_->robot_velocity();
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
@@ -253,7 +118,6 @@
}
}
-
if (data.IsPressed(kVisionAlign)) {
// Align shot using vision
// TODO(campbell): Add vision aligning.
@@ -441,10 +305,6 @@
double hood_goal_ = 0.3;
double shooter_velocity_ = 0.0;
- // Goals to send to the drivetrain in closed loop mode.
- double left_goal_ = 0.0;
- double right_goal_ = 0.0;
-
bool was_running_ = false;
bool auto_running_ = false;
diff --git a/y2017_bot3/BUILD b/y2017_bot3/BUILD
index 9ff13fd..9fc50f8 100644
--- a/y2017_bot3/BUILD
+++ b/y2017_bot3/BUILD
@@ -1,6 +1,27 @@
load('/aos/downloader/downloader', 'aos_downloader')
cc_binary(
+ name = 'joystick_reader',
+ srcs = [
+ 'joystick_reader.cc',
+ ],
+ deps = [
+ '//aos/common/actions:action_lib',
+ '//aos/common/logging',
+ '//aos/common/util:log_interval',
+ '//aos/common:time',
+ '//aos/input:drivetrain_input',
+ '//aos/input:joystick_input',
+ '//aos/linux_code:init',
+ '//frc971/autonomous:auto_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/autonomous:base_autonomous_actor',
+ '//y2017_bot3/control_loops/superstructure:superstructure_queue',
+ '//y2017_bot3/control_loops/superstructure:superstructure_lib',
+ ],
+)
+
+cc_binary(
name = 'wpilib_interface',
srcs = [
'wpilib_interface.cc',
@@ -43,6 +64,7 @@
start_srcs = [
'//aos:prime_start_binaries',
'//y2017_bot3/control_loops/drivetrain:drivetrain',
+ '//y2017_bot3/control_loops/superstructure:superstructure',
':wpilib_interface',
],
srcs = [
@@ -56,6 +78,7 @@
start_srcs = [
'//aos:prime_start_binaries_stripped',
'//y2017_bot3/control_loops/drivetrain:drivetrain.stripped',
+ '//y2017_bot3/control_loops/superstructure:superstructure.stripped',
':wpilib_interface.stripped',
],
srcs = [
diff --git a/y2017_bot3/joystick_reader.cc b/y2017_bot3/joystick_reader.cc
new file mode 100644
index 0000000..40a8d12
--- /dev/null
+++ b/y2017_bot3/joystick_reader.cc
@@ -0,0 +1,145 @@
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/input/drivetrain_input.h"
+#include "aos/input/joystick_input.h"
+#include "aos/linux_code/init.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::y2017_bot3::control_loops::superstructure_queue;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::ControlBit;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::POVLocation;
+using ::aos::input::DrivetrainInputReader;
+
+namespace y2017_bot3 {
+namespace input {
+namespace joysticks {
+
+const ButtonLocation kHangerOn(3, 1);
+const ButtonLocation kRollersOn(3, 2);
+const ButtonLocation kGearOut(3, 3);
+
+const ButtonLocation kRollerOn(3, 4);
+
+std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader() {
+ drivetrain_input_reader_ = DrivetrainInputReader::Make(
+ DrivetrainInputReader::InputType::kSteeringWheel);
+ }
+
+ void RunIteration(const ::aos::input::driver_station::Data &data) override {
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+
+ const bool last_auto_running = auto_running_;
+ auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
+ data.GetControlBit(ControlBit::kEnabled);
+ if (auto_running_ != last_auto_running) {
+ if (auto_running_) {
+ StartAuto();
+ } else {
+ StopAuto();
+ }
+ }
+
+ if (!auto_running_) {
+ HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+
+ // Process pending actions.
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ drivetrain_input_reader_->HandleDrivetrain(data);
+ robot_velocity_ = drivetrain_input_reader_->robot_velocity();
+ }
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ superstructure_queue.status.FetchLatest();
+ if (!superstructure_queue.status.get()) {
+ LOG(ERROR, "Got no superstructure status packet.\n");
+ return;
+ }
+
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ new_superstructure_goal->voltage_rollers = 0.0;
+
+ if (data.IsPressed(kRollerOn)) {
+ new_superstructure_goal->voltage_rollers = 12.0;
+ }
+
+ if (data.IsPressed(kHangerOn)) {
+ new_superstructure_goal->hanger_voltage = 12.0;
+ }
+
+
+ if (data.IsPressed(kGearOut)) {
+ new_superstructure_goal->fingers_out = true;
+ } else {
+ new_superstructure_goal->fingers_out = false;
+ }
+
+ LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
+
+ private:
+ void StartAuto() {
+ LOG(INFO, "Starting auto mode\n");
+ ::frc971::autonomous::AutonomousActionParams params;
+ ::frc971::autonomous::auto_mode.FetchLatest();
+ if (::frc971::autonomous::auto_mode.get() != nullptr) {
+ params.mode = ::frc971::autonomous::auto_mode->mode;
+ } else {
+ LOG(WARNING, "no auto mode values\n");
+ params.mode = 0;
+ }
+ action_queue_.EnqueueAction(
+ ::frc971::autonomous::MakeAutonomousAction(params));
+ }
+
+ void StopAuto() {
+ LOG(INFO, "Stopping auto mode\n");
+ action_queue_.CancelAllActions();
+ }
+ double robot_velocity_ = 0.0;
+ ::aos::common::actions::ActionQueue action_queue_;
+
+ bool was_running_ = false;
+ bool auto_running_ = false;
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2017_bot3
+
+int main() {
+ ::aos::Init(-1);
+ ::y2017_bot3::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}