Reorganize directory structure for third robot.
I added a "bot3" directory, so that we can have all the third
robot specific stuff live there.
Most of these changes were just copying files from frc971. Many
of the files have no modifications except changed paths in the
include directives, changed paths in gyp files, and changed
namespace names. Some of the code is new, however, for instance,
I modified motor_writer to control the motors and pistons on
the third robot, as well as added a new "rollers" queue to
transfer data pertaining to these.
I also updated the build system so that builds for the normal
robot code and the third robot code would in no way conflict
with each other. Finally, I figured out how I want to handle
the constants, although I haven't put actual values for
that in yet.
I cleaned up the python code, adding in the small changes to
that which Comran wanted.
Change-Id: I04e17dc8b17a980338b718a78e348ea647ec060b
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
new file mode 100644
index 0000000..20cb7d3
--- /dev/null
+++ b/bot3/input/input.gyp
@@ -0,0 +1,41 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'joystick_reader',
+ 'type': 'executable',
+ 'sources': [
+ 'joystick_reader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/prime/input/input.gyp:joystick_input',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:log_interval',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
+ ],
+ },
+ {
+ 'target_name': 'sensor_receiver',
+ 'type': 'executable',
+ 'sources': [
+ 'sensor_receiver.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_constants',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/queues/queues.gyp:queues',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:wrapping_counter',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:sensor_reader',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/controls/controls.gyp:output_check',
+ ],
+ },
+ ],
+}
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
new file mode 100644
index 0000000..9156509
--- /dev/null
+++ b/bot3/input/joystick_reader.cc
@@ -0,0 +1,613 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/prime/input/joystick_input.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#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 "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"
+
+using ::frc971::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 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 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_;
+};
+
+
+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) {}
+
+ virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
+ if (data.GetControlBit(ControlBit::kAutonomous)) {
+ if (data.PosEdge(ControlBit::kEnabled)){
+ LOG(INFO, "Starting auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(true)
+ .Send();
+ } else if (data.NegEdge(ControlBit::kEnabled)) {
+ LOG(INFO, "Stopping auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .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");
+ }
+ }
+ }
+ } else {
+ HandleTeleop(data);
+ }
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ bool is_control_loop_driving = false;
+ double left_goal = 0.0;
+ double right_goal = 0.0;
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+ 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;
+ if (data.PosEdge(kDriveControlLoopEnable1) ||
+ data.PosEdge(kDriveControlLoopEnable2)) {
+ if (drivetrain.position.FetchLatest() && gyro_reading.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.position->right_encoder) /
+ 2.0 -
+ throttle * kThrottleGain / 2.0;
+ angle = gyro_reading->angle;
+ filtered_goal_distance = distance;
+ }
+ }
+ is_control_loop_driving = true;
+
+ // const double gyro_angle = Gyro.View().angle;
+ const double goal_theta = angle - wheel * 0.27;
+ const double goal_distance = distance + throttle * kThrottleGain;
+ const double robot_width = 22.0 / 100.0 * 2.54;
+ const double kMaxVelocity = 0.6;
+ if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance += kMaxVelocity * 0.02;
+ } else if (goal_distance <
+ -kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance -= kMaxVelocity * 0.02;
+ } else {
+ filtered_goal_distance = goal_distance;
+ }
+ left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
+ right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
+ is_high_gear_ = false;
+
+ LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
+ }
+ if (!drivetrain.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear_)
+ .quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal)
+ .right_goal(right_goal)
+ .left_velocity_goal(0)
+ .right_velocity_goal(0)
+ .Send()) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+ if (data.PosEdge(kShiftHigh)) {
+ is_high_gear_ = false;
+ }
+ if (data.PosEdge(kShiftLow)) {
+ is_high_gear_ = true;
+ }
+ }
+
+ 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");
+ }
+ if (data.IsPressed(kRollersIn) || data.IsPressed(kRollersOut)) {
+ intake_power_ = 0.0;
+ separation_angle_ = kGrabSeparation;
+ moving_for_shot_ = false;
+ }
+
+ 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
+
+int main() {
+ ::aos::Init();
+ ::frc971::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/bot3/input/sensor_receiver.cc b/bot3/input/sensor_receiver.cc
new file mode 100644
index 0000000..6aec89d
--- /dev/null
+++ b/bot3/input/sensor_receiver.cc
@@ -0,0 +1,160 @@
+#include <inttypes.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/wrapping_counter.h"
+#include "aos/common/time.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/controls/output_check.q.h"
+
+#include "bbb/sensor_reader.h"
+
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
+#include "bot3/queues/to_log.q.h"
+#include "frc971/queues/other_sensors.q.h"
+#include "frc971/shifter_hall_effect.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::bot3::control_loops::drivetrain;
+using ::frc971::sensors::gyro_reading;
+using ::aos::util::WrappingCounter;
+
+namespace bot3 {
+namespace {
+
+// TODO (danielp): Find out the real ratios here.
+double drivetrain_translate(int32_t in) {
+ return static_cast<double>(in)
+ / (256.0 /*cpr*/ * 4.0 /*quad*/)
+ * (18.0 / 50.0 /*output stage*/) * (56.0 / 30.0 /*encoder gears*/)
+ // * constants::GetValues().drivetrain_encoder_ratio
+ * (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
+static const double kVcc = 5.15;
+
+// Translates values from the ADC into voltage.
+double adc_translate(uint16_t in) {
+ if (false) {
+ // This is the simple theoretical math.
+ static const uint16_t kMaximumValue = 0x3FF;
+ static const double kR1 = 5, kR2 = 6.65;
+ const double raw =
+ (kVcc * static_cast<double>(in) / static_cast<double>(kMaximumValue));
+ return (raw * (kR1 + kR2) - (kVcc / 2) * kR2) / kR1;
+ } else {
+ // This is from a linear regression calculated with some actual data points.
+ static const double kM = 0.012133, kB = -3.6813;
+ return static_cast<double>(in) * kM + kB;
+ }
+}
+
+double battery_translate(uint16_t in_high, uint16_t in_low) {
+ const double high = adc_translate(in_high), low = adc_translate(in_low);
+ static const double kDividerBig = 5.55, kDividerSmall = 2.66;
+ return (high - low) * (kDividerBig + kDividerSmall) / kDividerSmall +
+ kDividerBig / kDividerSmall * kVcc;
+}
+
+double gyro_translate(int64_t in) {
+ return in / 16.0 / 1000.0 / (180.0 / M_PI);
+}
+
+double hall_translate(const ::frc971::constants::ShifterHallEffect &k, uint16_t in_low,
+ uint16_t in_high) {
+ const double low_ratio =
+ 0.5 * (in_low - static_cast<double>(k.low_gear_low)) /
+ static_cast<double>(k.low_gear_middle - k.low_gear_low);
+ const double high_ratio =
+ 0.5 + 0.5 * (in_high - static_cast<double>(k.high_gear_middle)) /
+ static_cast<double>(k.high_gear_high - k.high_gear_middle);
+
+ // Return low when we are below 1/2, and high when we are above 1/2.
+ if (low_ratio + high_ratio < 1.0) {
+ return low_ratio;
+ } else {
+ return high_ratio;
+ }
+}
+
+void PacketReceived(const ::bbb::DataStruct *data,
+ const ::aos::time::Time &cape_timestamp) {
+ ::aos::time::TimeFreezer time_freezer;
+
+ ::frc971::logging_structs::CapeReading reading_to_log(
+ cape_timestamp, static_cast<uint16_t>(sizeof(*data)),
+ data->main.low_left_drive_hall, data->main.high_left_drive_hall,
+ data->main.low_right_drive_hall, data->main.high_right_drive_hall);
+ LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
+ bool bad_gyro;
+ // TODO(brians): Switch to LogInterval for these things.
+ if (data->uninitialized_gyro) {
+ LOG(DEBUG, "uninitialized gyro\n");
+ bad_gyro = true;
+ } else if (data->zeroing_gyro) {
+ LOG(DEBUG, "zeroing gyro\n");
+ bad_gyro = true;
+ } else if (data->bad_gyro) {
+ LOG(ERROR, "bad gyro\n");
+ bad_gyro = true;
+ } else if (data->old_gyro_reading) {
+ LOG(WARNING, "old/bad gyro reading\n");
+ bad_gyro = true;
+ } else {
+ bad_gyro = false;
+ }
+
+ if (!bad_gyro) {
+ gyro_reading.MakeWithBuilder()
+ .angle(gyro_translate(data->gyro_angle))
+ .Send();
+ }
+
+ if (data->analog_errors != 0) {
+ LOG(WARNING, "%" PRIu8 " analog errors\n", data->analog_errors);
+ }
+
+ if (data->main.output_check_pulse_length != 0) {
+ auto message = ::aos::controls::output_check_received.MakeMessage();
+ // TODO(brians): Fix this math to match what the cRIO actually does.
+ // It's close but not quite right.
+ message->pulse_length =
+ static_cast<double>(data->main.output_check_pulse_length) / 10000.0;
+ if (message->pulse_length > 2.7) {
+ LOG(WARNING, "insane PWM pulse length %fms\n", message->pulse_length);
+ } else {
+ message->pwm_value = (message->pulse_length - 0.5) / 2.0 * 255.0 + 0.5;
+ LOG_STRUCT(DEBUG, "received", *message);
+ message.Send();
+ }
+ }
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data->main.right_drive))
+ .left_encoder(-drivetrain_translate(data->main.left_drive))
+ .left_shifter_position(hall_translate(control_loops::kBot3LeftDriveShifter,
+ data->main.low_left_drive_hall,
+ data->main.high_left_drive_hall))
+ .right_shifter_position(hall_translate(control_loops::kBot3RightDriveShifter,
+ data->main.low_right_drive_hall,
+ data->main.high_right_drive_hall))
+ .battery_voltage(battery_translate(data->main.battery_voltage_high,
+ data->main.battery_voltage_low))
+ .Send();
+}
+
+} // namespace
+} // namespace bot3
+
+int main() {
+ ::aos::Init(::bbb::SensorReader::kRelativePriority);
+ ::bbb::SensorReader reader("comp");
+ while (true) {
+ ::bot3::PacketReceived(reader.ReadPacket(), reader.GetCapeTimestamp());
+ }
+ ::aos::Cleanup();
+}