commit e20b4616fce8c49f83e7b50a37a42879b82b3911
author Austin Schuh <austin.linux@gmail.com> Fri Feb 28 22:40:54 2020 -0800
committer Gerrit Code Review <gerrit@robotics.mvla.net> Fri Feb 28 22:40:54 2020 -0800
tree b255011b087a53bc0e0f4aabee589b099241cb7a
parent 6f3640a90151904d19b394feb767e6dee1fce802
parent 1039e43689b05f913930301ac59def7fdf5d9315

Merge "Add climber to wpilib interface"

y2020/joystick_reader.cc

diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 89de4b9..75a4b26 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -43,7 +43,9 @@
 const ButtonLocation kFeed(4, 1);
 const ButtonLocation kIntakeExtend(3, 9);
 const ButtonLocation kIntakeIn(4, 4);
-const ButtonLocation kSpit(4, 5);
+const ButtonLocation kSpit(4, 3);
+
+const ButtonLocation kWinch(3, 14);
 
 class Reader : public ::aos::input::ActionJoystickInput {
  public:
@@ -75,6 +77,7 @@
     float roller_speed = 0.0f;
     double accelerator_speed = 0.0;
     double finisher_speed = 0.0;
+    double climber_speed = 0.0;
 
     if (data.IsPressed(kTurret)) {
       turret_pos = 3.5;
@@ -105,6 +108,10 @@
       roller_speed = -6.0f;
     }
 
+    if (data.IsPressed(kWinch)) {
+      climber_speed = 12.0f;
+    }
+
     auto builder = superstructure_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<superstructure::Goal> superstructure_goal_offset;
@@ -137,6 +144,7 @@
       superstructure_goal_builder.add_roller_voltage(roller_speed);
       superstructure_goal_builder.add_shooter(shooter_offset);
       superstructure_goal_builder.add_shooting(data.IsPressed(kFeed));
+      superstructure_goal_builder.add_climber_voltage(climber_speed);
 
       if (!builder.Send(superstructure_goal_builder.Finish())) {
         AOS_LOG(ERROR, "Sending superstructure goal failed.\n");

y2020/wpilib_interface.cc

diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 91182fc..8c30918 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -427,7 +427,7 @@
     if (climber_falcon_) {
       climber_falcon_->Set(
           ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
-          std::clamp(output.climber_voltage(), -kMaxBringupPower,
+          std::clamp(-output.climber_voltage(), -kMaxBringupPower,
                      kMaxBringupPower) /
               12.0);
     }
@@ -550,8 +550,8 @@
         make_unique<::frc::TalonFX>(4));
     superstructure_writer.set_flywheel_falcon(make_unique<::frc::TalonFX>(9));
     // TODO: check port
-    //superstructure_writer.set_climber_falcon(
-        //make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(11));
+    superstructure_writer.set_climber_falcon(
+        make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
 
     AddLoop(&output_event_loop);