From d9b473c0653b72cf54d0170ccc687f694b2db063 Mon Sep 17 00:00:00 2001 From: j a Date: Sat, 25 Feb 2023 14:48:57 -0600 Subject: [PATCH] feature(elevarm): Arm and Wrist CANcoder within range on dashboard fix: pit mode greyed out --- scripts/.gitignore | 1 + src/main/cpp/subsystems/Elevarm.cpp | 51 +++++++++++++++++++++++---- src/main/include/Constants.h | 1 + src/main/include/subsystems/Elevarm.h | 13 +++++-- src/main/include/subsystems/Intake.h | 3 -- 5 files changed, 57 insertions(+), 12 deletions(-) diff --git a/scripts/.gitignore b/scripts/.gitignore index 564d0dcc..3793ab86 100644 --- a/scripts/.gitignore +++ b/scripts/.gitignore @@ -1 +1,2 @@ *backup/ +pyScripts/ \ No newline at end of file diff --git a/src/main/cpp/subsystems/Elevarm.cpp b/src/main/cpp/subsystems/Elevarm.cpp index 574e8c27..ee87fdf6 100644 --- 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 index b86a10c4..3c230e02 100644 --- 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 index d6e7bf06..8221adf1 100644 --- 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; }; \ No newline at end of file diff --git a/src/main/include/subsystems/Intake.h b/src/main/include/subsystems/Intake.h index 9db14fdf..6e66c79f 100644 --- 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; }; \ No newline at end of file