feature(elevarm): Arm and Wrist CANcoder within range on dashboard

fix: pit mode greyed out
Valor6800 · Feb 25, 2023 · d9b473c · d9b473c
1 parent 261aa7b
commit d9b473c
Show file tree

Hide file tree

Showing 5 changed files with 57 additions and 12 deletions.
diff --git a/scripts/.gitignore b/scripts/.gitignore
@@ -1 +1,2 @@
 *backup/
+pyScripts/
diff --git a/src/main/cpp/subsystems/Elevarm.cpp b/src/main/cpp/subsystems/Elevarm.cpp
@@ -20,6 +20,11 @@
 #define WRIST_FORWARD_LIMIT 180.0f
 #define WRIST_REVERSE_LIMIT -180.0f
 
+#define WRIST_HIGH_EDGE 360.0f
+#define WRIST_LOW_EDGE 0.0f
+#define ARM_HIGH_EDGE 100.0f
+#define ARM_LOW_EDGE  0.0f
+
 #define CANCODER_OFFSET 114.961f
 
 #define CARRIAGE_K_F 0.000156f  
@@ -80,6 +85,7 @@ Elevarm::Elevarm(frc::TimedRobot *_robot, Intake *_intake) : ValorSubsystem(_rob
                             armRotateMotor(CANIDs::ARM_ROTATE, ValorNeutralMode::Coast, false, "baseCAN"),
                             armCANcoder(CANIDs::ARM_CANCODER, "baseCAN"),
                             wristMotor(CANIDs::WRIST, ValorNeutralMode::Brake, false, "baseCAN"),
+                            wristCANcoder(CANIDs::WRIST_CANCODER, "baseCAN"),
                             manualMaxArmSpeed(MAN_MAX_ROTATE),
                             manualMaxCarriageSpeed(MAN_MAX_CARRIAGE),
                             carriageStallPower(P_MIN_CARRIAGE)
@@ -150,6 +156,9 @@ void Elevarm::init()
 
     armCANcoder.ConfigAbsoluteSensorRange(AbsoluteSensorRange::Unsigned_0_to_360);
     armCANcoder.SetPositionToAbsolute();
+
+    wristCANcoder.ConfigAbsoluteSensorRange(AbsoluteSensorRange::Unsigned_0_to_360);
+    wristCANcoder.SetPositionToAbsolute();
 
     wristMotor.setConversion(1.0 / WRIST_GEAR_RATIO * 360.0);
     wristMotor.setForwardLimit(WRIST_FORWARD_LIMIT);
@@ -183,17 +192,19 @@ void Elevarm::init()
     posMap[ElevarmPieceState::ELEVARM_CUBE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_MID] =frc::Pose2d(-0.904_m, 1.09_m, -180.0_deg);
     posMap[ElevarmPieceState::ELEVARM_CUBE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_HIGH] =frc::Pose2d(-0.904_m, 1.09_m, -180.0_deg);
     posMap[ElevarmPieceState::ELEVARM_CUBE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_SNAKE] =frc::Pose2d(-0.904_m, 1.09_m, -180.0_deg);
-
+
+    resetState();
+    armRotateMotor.setEncoderPosition((getArmCANcoderPosition() - CANCODER_OFFSET) / CANCODER_GEAR_RATIO );
+    carriageMotors.setEncoderPosition(0.8352);
+    wristMotor.setEncoderPosition(0.0);
 
     table->PutNumber("Carriage Max Manual Speed", manualMaxCarriageSpeed);
     table->PutNumber("Arm Rotate Max Manual Speed", manualMaxArmSpeed);
     table->PutBoolean("Pit Mode", futureState.pitModeEnabled);
     table->PutNumber("Carraige Stall", carriageStallPower);
 
-    resetState();
-    armRotateMotor.setEncoderPosition((armCANcoder.GetAbsolutePosition() - CANCODER_OFFSET) / CANCODER_GEAR_RATIO );
-    carriageMotors.setEncoderPosition(0.8352);
-    wristMotor.setEncoderPosition(0.0);
+    wristInRange = getWristCANcoderPosition() < WRIST_HIGH_EDGE && getWristCANcoderPosition() > WRIST_LOW_EDGE;
+    armInRange = getArmCANcoderPosition() < ARM_HIGH_EDGE && getArmCANcoderPosition() > ARM_LOW_EDGE;
 }
 
 void Elevarm::assessInputs()
@@ -258,6 +269,9 @@ void Elevarm::analyzeDashboard()
     futureState.resultKinematics = forwardKinematics(Elevarm::Positions(carriageMotors.getPosition(), armRotateMotor.getPosition(), wristMotor.getPosition()));
     futureState.frontMinAngle = minAngle(true);
     futureState.backMinAngle = minAngle(false);
+
+    wristInRange = getWristCANcoderPosition() < WRIST_HIGH_EDGE && getWristCANcoderPosition() > WRIST_LOW_EDGE;
+    armInRange = getArmCANcoderPosition() < ARM_HIGH_EDGE && getArmCANcoderPosition() > ARM_LOW_EDGE;
 }
 
 void Elevarm::assignOutputs()
@@ -424,6 +438,16 @@ frc::Pose2d Elevarm::forwardKinematics(Elevarm::Positions positions)
     return frc::Pose2d((units::length::meter_t)x,(units::length::meter_t)z,(units::angle::degree_t)w);
 }
 
+double Elevarm::getArmCANcoderPosition()
+{
+    return armCANcoder.GetAbsolutePosition();
+}
+
+double Elevarm::getWristCANcoderPosition()
+{
+    return wristCANcoder.GetAbsolutePosition();
+}
+
 void Elevarm::InitSendable(wpi::SendableBuilder& builder)
 {
     builder.SetSmartDashboardType("Subsystem");
@@ -524,7 +548,22 @@ void Elevarm::InitSendable(wpi::SendableBuilder& builder)
     );
     builder.AddDoubleProperty(
         "Arm CANcoder",
-        [this]{ return armCANcoder.GetAbsolutePosition(); },
+        [this]{ return getArmCANcoderPosition(); },
+        nullptr
+    );
+    builder.AddDoubleProperty(
+        "Wrist CANcoder",
+        [this]{ return getWristCANcoderPosition(); },
+        nullptr
+    );
+    builder.AddBooleanProperty(
+        "Wrist CANcoder in range",
+        [this]{ return wristInRange; },
+        nullptr
+    );
+    builder.AddBooleanProperty(
+        "Arm CANcoder in range",
+        [this]{ return armInRange; },
         nullptr
     );
 }

diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h
@@ -55,4 +55,5 @@ namespace CANIDs {
     constexpr static int ARM_ROTATE = 11;
     constexpr static int ARM_CANCODER = 14;
     constexpr static int WRIST = 15;
+    constexpr static int WRIST_CANCODER = 16;
 }
diff --git a/src/main/include/subsystems/Elevarm.h b/src/main/include/subsystems/Elevarm.h
@@ -61,6 +61,9 @@ class Elevarm : public ValorSubsystem
 
      void InitSendable(wpi::SendableBuilder& builder);
 
+    double getArmCANcoderPosition();
+    double getWristCANcoderPosition();
+
     struct Positions {
         Positions() {
             Positions(0,0,0);
@@ -175,6 +178,7 @@ class Elevarm : public ValorSubsystem
      ValorFalconController armRotateMotor;
 
     ctre::phoenix::sensors::WPI_CANCoder armCANcoder;
+    ctre::phoenix::sensors::WPI_CANCoder wristCANcoder;
 
      ValorFalconController wristMotor;
 
@@ -187,7 +191,10 @@ class Elevarm : public ValorSubsystem
 
     Intake *intake;
 
-     double manualMaxCarriageSpeed;
-     double manualMaxArmSpeed;
-     double carriageStallPower;
+    double manualMaxCarriageSpeed;
+    double manualMaxArmSpeed;
+    double carriageStallPower;
+
+    bool wristInRange;
+    bool armInRange;
 };
diff --git a/src/main/include/subsystems/Intake.h b/src/main/include/subsystems/Intake.h
@@ -109,8 +109,5 @@ class Intake : public ValorSubsystem
 private:
 
      ValorFalconController intakeMotor;
-
-     ValorFalconController wristMotor;
-
      ValorCurrentSensor currySensor;
 };