Convert midbar climb to control outer hooks instead of inner

- Nick Q

Author: PhyXTGears-1720

src/main/cpp/Robot.cpp

@@ -38,55 +38,56 @@ void Robot::RobotInit()
     mRunIntakeCommand = new RunIntakeCommand(mIntake);
     mShootCommand = new ShootCommand(mShooter);
 
-    mManualRetractInnerArms = new frc2::FunctionalCommand(
+    mManualRetractOuterArms = new frc2::FunctionalCommand(
         [&]() {},
         [&]() {
-            bool isInner1NearTarget = mClimber->isInner1NearTarget(0.0);
-            bool isInner2NearTarget = mClimber->isInner2NearTarget(0.0);
+            bool isOuter1NearTarget = mClimber->isOuter1NearTarget(0.0);
+            bool isOuter2NearTarget = mClimber->isOuter2NearTarget(0.0);
 
-            if (isInner1NearTarget) {
-                mClimber->stopInner1();
+            if (isOuter1NearTarget) {
+                mClimber->stopOuter1();
             } else {
-                mClimber->runInner1(0.2);
+                mClimber->runOuter1(0.4);
             }
 
-            if (isInner2NearTarget) {
-                mClimber->stopInner2();
+            if (isOuter2NearTarget) {
+                mClimber->stopOuter2();
             } else {
-                mClimber->runInner2(0.2);
+                mClimber->runOuter2(0.4);
             }
         },
         [&](bool) {
-            mClimber->stopInner1();
-            mClimber->stopInner2();
+            mClimber->stopOuter1();
+            mClimber->stopOuter2();
         },
-        [&]() { return mClimber->isInner1NearTarget(0.0) || mClimber->isInner2NearTarget(0.0); },
+        [&]() { return mClimber->isOuter1NearTarget(0.0) || mClimber->isOuter2NearTarget(0.0); },
         { mClimber }
     );
 
-    mManualExtendInnerArms = new frc2::FunctionalCommand(
+    mManualExtendOuterArms = new frc2::FunctionalCommand(
         [&]() {},
         [&]() {
-            bool isInner1NearTarget = mClimber->isInner1NearTarget(20.0);
-            bool isInner2NearTarget = mClimber->isInner2NearTarget(20.0);
+            bool isOuter1NearTarget = mClimber->isOuter1NearTarget(20.0);
+            bool isOuter2NearTarget = mClimber->isOuter2NearTarget(20.0);
 
-            if (isInner1NearTarget) {
-                mClimber->stopInner1();
+            if (isOuter1NearTarget) {
+                mClimber->stopOuter1();
             } else {
-                mClimber->runInner1(-0.2);
+                mClimber->runOuter1(-0.4);
             }
 
-            if (isInner2NearTarget) {
-                mClimber->stopInner2();
+            if (isOuter2NearTarget) {
+                mClimber->stopOuter2();
             } else {
-                mClimber->runInner2(-0.2);
+                mClimber->runOuter2(-0.4);
             }
         },
         [&](bool) {
-            mClimber->stopInner1();
-            mClimber->stopInner2();
+            mClimber->stopOuter1();
+            mClimber->stopOuter2();
         },
-        [&]() { return mClimber->isInner1NearTarget(20.0) || mClimber->isInner2NearTarget(20.0); },
+        
+        [&]() { return mClimber->isOuter1NearTarget(20.0) || mClimber->isOuter2NearTarget(20.0); },
         { mClimber }
     );
 
@@ -226,14 +227,14 @@ void Robot::TeleopPeriodic()
     double opY = operatorController->GetLeftY();
     opY = fabs(opY) < 0.3 ? 0.0 : opY;
     if (opY < 0.0) {
-        mManualRetractInnerArms->Schedule();
+        mManualRetractOuterArms->Schedule();
     } else if (opY > 0.0) {
-        mManualExtendInnerArms->Schedule();
+        mManualExtendOuterArms->Schedule();
     } else {
-        if (mManualRetractInnerArms->IsScheduled()) {
-            mManualRetractInnerArms->Cancel();
-        } else if (mManualExtendInnerArms->IsScheduled()) {
-            mManualExtendInnerArms->Cancel();
+        if (mManualRetractOuterArms->IsScheduled()) {
+            mManualRetractOuterArms->Cancel();
+        } else if (mManualExtendOuterArms->IsScheduled()) {
+            mManualExtendOuterArms->Cancel();
         }
     }
 }

src/main/cpp/climber/climber.cpp

@@ -139,6 +139,14 @@ void Climber::runInner2(double speed) {
     mInnerHookMotor2.Set(speed);
 }
 
+void Climber::runOuter1(double speed) {
+    mOuterHookMotor1.Set(speed);
+}
+
+void Climber::runOuter2(double speed) {
+    mOuterHookMotor2.Set(speed);
+}
+
 void Climber::lockArms() {
     mStopServo1.Set(config.servo1.lockPosition);
     mStopServo2.Set(config.servo2.lockPosition);

src/main/cpp/commands/climber/ClimbMidBarOnly.cpp

@@ -3,6 +3,11 @@
 #include "commands/climber/ExtendInnerArms.h"
 #include "commands/climber/RetractInnerArms.h"
 #include "commands/climber/RotateInnerArms.h"
+
+#include "commands/climber/ExtendOuterArms.h"
+#include "commands/climber/RetractOuterArms.h"
+#include "commands/climber/RotateOuterArms.h"
+
 #include "commands/climber/LockArms.h"
 
 #include <frc2/command/InstantCommand.h>
@@ -18,23 +23,21 @@ ClimbMidBarOnly::ClimbMidBarOnly(Climber * climber, std::shared_ptr<cpptoml::tab
     mReachMidBar = new frc2::SequentialCommandGroup(
         frc2::InstantCommand {
             [=]() {
-                climber->unlockArms();
-                climber->setInnerUnderLoad(false);
+                climber->setOuterUnderLoad(false);
             },
             { climber }
         },
-        RotateInnerArmsCommand { climber, config.verticalArmAngle },
-        ExtendInnerArmsCommand { climber, config.initialExtension }
+        // RotateOuterArmsCommand { climber, config.verticalArmAngle },
+        ExtendOuterArmsCommand { climber, config.initialExtension }
     );
 
     mClimbMidBarAndLock = new frc2::SequentialCommandGroup(
         frc2::InstantCommand {
             [=] () {
-                climber->setInnerUnderLoad(true);
+                climber->setOuterUnderLoad(true);
             },
             { climber }
         },
-        RetractInnerArmsCommand { climber, config.liftRetraction },
-        LockArmsCommand { climber }
+        RetractOuterArmsCommand { climber, config.liftRetraction }
     );
 }

src/main/deploy/config.toml

@@ -23,13 +23,13 @@ servo1.lockPosition = 0.2
 servo2.unlockPosition = 0.65
 servo2.lockPosition = 1.0
 
-extendSpeed = 0.45
-retractSpeed = -0.45
+extendSpeed = -0.45
+retractSpeed = 0.45
 inchesPerRevolution = 0.128325 # equal to 2.5665 / 20, meaning 2.5665 inches per 20 revolutions
 innerStaticFriction = 0.05 # actual value unknown
-outerStaticFriction = 0.00 # actual value unknown
+outerStaticFriction = 0.05 # actual value unknown
 innerStaticFrictionWithLoad = 0.25 # actual value unknown
-outerStaticFrictionWithLoad = 0.0 # actual value unknown
+outerStaticFrictionWithLoad = 0.25 # actual value unknown
 
 [command.climb.midbar]
 initialExtension = 16.0

src/main/include/Robot.h

@@ -66,8 +66,8 @@ class Robot : public frc::TimedRobot
     ShootCommand *mShootCommand = nullptr;
     RunIntakeCommand *mRunIntakeCommand = nullptr;
 
-    frc2::FunctionalCommand *mManualRetractInnerArms = nullptr;
-    frc2::FunctionalCommand *mManualExtendInnerArms = nullptr;
+    frc2::FunctionalCommand *mManualRetractOuterArms = nullptr;
+    frc2::FunctionalCommand *mManualExtendOuterArms = nullptr;
 
     frc2::StartEndCommand *mShootNear = nullptr;
     frc2::StartEndCommand *mShootFar = nullptr;

src/main/include/climber/climber.h

@@ -30,6 +30,8 @@ class Climber : public frc2::SubsystemBase{
         void stopInner2();
         void runInner1(double speed);
         void runInner2(double speed);
+        void runOuter1(double speed);
+        void runOuter2(double speed);
 
         void lockArms();
         void unlockArms();