Joystick reader now has good enough controls to drive.
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index d751a43..be49798 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -30,17 +30,42 @@
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kClawClosed(3, 7);
-const ButtonLocation kClawOpen(3, 9);
+
const ButtonLocation kFire(3, 11);
-const ButtonLocation kUnload(3, 12);
+const ButtonLocation kUnload(3, 1);
+const ButtonLocation kReload(3, 9);
+
+const ButtonLocation kRollersOut(3, 12);
+const ButtonLocation kRollersIn(3, 10);
+
+const ButtonLocation kTuck(3, 8);
+const ButtonLocation kIntakePosition(3, 7);
+
+const ButtonLocation kLongShot(3, 5);
+const ButtonLocation kMediumShot(3, 3);
+const ButtonLocation kShortShot(3, 6);
+
+struct ClawGoal {
+ double angle;
+ double separation;
+};
+
+const ClawGoal kTuckGoal = {-2.273474, -0.749484};
+const ClawGoal kIntakeGoal = {-2.273474, 0.0};
+
+const ClawGoal kLongShotGoal = {-M_PI / 2.0 + 0.5, 0.05};
+const ClawGoal kMediumShotGoal = {-0.8, 0.05};
+const ClawGoal kShortShotGoal = {-0.2, 0.05};
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() : closed_(true) {}
+ Reader()
+ : is_high_gear_(false),
+ shot_power_(0.1),
+ goal_angle_(0.0),
+ separation_angle_(0.0) {}
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
- static bool is_high_gear = false;
if (data.GetControlBit(ControlBit::kAutonomous)) {
if (data.PosEdge(ControlBit::kEnabled)){
@@ -52,92 +77,128 @@
::frc971::autonomous::autonomous.MakeWithBuilder()
.run_auto(false).Send();
}
- } else { // teleop
- 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))) {
- 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.FetchLatest()) {
- distance = (drivetrain.position->left_encoder +
- drivetrain.position->right_encoder) / 2.0
- - throttle * kThrottleGain / 2.0;
- angle = gyro->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).Send()) {
- LOG(WARNING, "sending stick values failed\n");
- }
-
- if (data.PosEdge(kShiftHigh)) {
- is_high_gear = false;
- }
- if (data.PosEdge(kShiftLow)) {
- is_high_gear = true;
- }
- if (data.PosEdge(kClawClosed)) {
- closed_ = true;
- }
- if (data.PosEdge(kClawOpen)) {
- closed_ = false;
- }
-
- double separation_angle = closed_ ? 0.0 : 0.5;
- double goal_angle = closed_ ? 0.0 : -0.2;
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(goal_angle)
- .separation_angle(separation_angle)
- .intake(false)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- }
- if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
- .shot_power(0.25)
- .shot_requested(data.IsPressed(kFire))
- .unload_requested(data.IsPressed(kUnload))
- .Send()) {
- LOG(WARNING, "sending 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.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.position->right_encoder) /
+ 2.0 -
+ throttle * kThrottleGain / 2.0;
+ angle = gyro->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)
+ .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;
+ }
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ HandleDrivetrain(data);
+
+ if (data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition)) {
+ SetGoal(kIntakeGoal);
+ } else if (data.IsPressed(kTuck)) {
+ SetGoal(kTuckGoal);
+ }
+
+ // TODO(austin): Wait for the claw to go to position before shooting, and
+ // open the claw as part of the actual fire step.
+ if (data.IsPressed(kLongShot)) {
+ shot_power_ = 0.25;
+ SetGoal(kLongShotGoal);
+ } else if (data.IsPressed(kMediumShot)) {
+ shot_power_ = 0.15;
+ SetGoal(kMediumShotGoal);
+ } else if (data.IsPressed(kShortShot)) {
+ shot_power_ = 0.07;
+ SetGoal(kShortShotGoal);
+ }
+
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(goal_angle_)
+ .separation_angle(separation_angle_)
+ .intake(data.IsPressed(kRollersIn)
+ ? 12.0
+ : (data.IsPressed(kRollersOut) ? -12.0 : 0.0))
+ .centering(data.IsPressed(kRollersIn) ? 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");
+ }
+ }
+
private:
- bool closed_;
+ bool is_high_gear_;
+ double shot_power_;
+ double goal_angle_;
+ double separation_angle_;
};
} // namespace joysticks