From 3be59ec721b10bcb1b8a9dc4ff7b767d85753b53 Mon Sep 17 00:00:00 2001 From: hahafoot Date: Fri, 2 Feb 2024 19:09:24 -0500 Subject: [PATCH] finished TestProject for testing motors on the robot. Done logging w/ encodrs and did sweeve. yay !!!!!! :3 Co-authored-by: naveed1117 Co-authored-by: jopy-man Co-authored-by: Lapcas --- .../com/stuypulse/robot/RobotContainer.java | 2 +- .../stuypulse/robot/subsystems/AmperTest.java | 15 +++++- .../robot/subsystems/ClimberTest.java | 20 +++++++- .../robot/subsystems/ConveyerTest.java | 20 +++++++- .../robot/subsystems/IntakeTest.java | 8 ++-- .../robot/subsystems/ShooterTest.java | 25 +++++++++- .../robot/subsystems/SwerveTest.java | 5 -- .../subsystems/swervetests/SwerveTest.java | 43 +++++++++++++++++ .../swervetests/module/SwerveModule.java | 9 ++++ .../swervetests/module/SwerveModuleTest.java | 48 +++++++++++++++++++ 10 files changed, 181 insertions(+), 14 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/SwerveTest.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/swervetests/SwerveTest.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/swervetests/module/SwerveModule.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/swervetests/module/SwerveModuleTest.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5b55349..dca0e6d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -46,7 +46,7 @@ private void configureDefaultCommands() {} private void configureButtonBindings() {} /**************/ - /*** AUTONS ***/ + /*** AUTONS ***/ /**************/ public void configureAutons() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/AmperTest.java b/src/main/java/com/stuypulse/robot/subsystems/AmperTest.java index 6897053..c29d855 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/AmperTest.java +++ b/src/main/java/com/stuypulse/robot/subsystems/AmperTest.java @@ -5,8 +5,11 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.stuypulse.robot.constants.Ports; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class AmperTest { + +public class AmperTest extends SubsystemBase { private final CANSparkMax scoreMotor; private final CANSparkMax liftMotor; private final RelativeEncoder liftEncoder; @@ -29,4 +32,14 @@ public void stop(){ scoreMotor.set(0); liftMotor.set(0); } + public double getLiftEncoder() { + return liftEncoder.getVelocity(); + } + @Override + public void periodic() { + SmartDashboard.putNumber("Amper/Intake Speed", scoreMotor.get()); + SmartDashboard.putNumber("Amper/Lift Speed", liftMotor.get()); + SmartDashboard.putNumber("Amper/Intake Current", scoreMotor.getOutputCurrent()); + SmartDashboard.putNumber("Amper/Lift Current", liftMotor.getOutputCurrent()); + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/ClimberTest.java b/src/main/java/com/stuypulse/robot/subsystems/ClimberTest.java index 567e376..856b8c4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/ClimberTest.java +++ b/src/main/java/com/stuypulse/robot/subsystems/ClimberTest.java @@ -5,7 +5,10 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.stuypulse.robot.constants.Ports; -public class ClimberTest{ +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ClimberTest extends SubsystemBase{ private final CANSparkMax leftMotor; private final CANSparkMax rightMotor; private final RelativeEncoder rightEncoder; @@ -30,6 +33,21 @@ public void stop() { rightMotor.set(0); } + public double getLeftEncoder(){ + return leftEncoder.getVelocity(); + } + + public double getRightEncoder(){ + return rightEncoder.getVelocity(); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Climber/Left Voltage", leftMotor.getAppliedOutput() * leftMotor.getBusVoltage()); + SmartDashboard.putNumber("Climber/Right Voltage", rightMotor.getAppliedOutput() * rightMotor.getBusVoltage()); + SmartDashboard.putNumber("Climber/Height", getRightEncoder()); + SmartDashboard.putNumber("Climber/Velocity", getLeftEncoder()); + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/ConveyerTest.java b/src/main/java/com/stuypulse/robot/subsystems/ConveyerTest.java index f379e9d..9b4a138 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/ConveyerTest.java +++ b/src/main/java/com/stuypulse/robot/subsystems/ConveyerTest.java @@ -5,7 +5,10 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.stuypulse.robot.constants.Ports; -public class ConveyerTest { +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ConveyerTest extends SubsystemBase{ private final CANSparkMax gandalfMotor; private final CANSparkMax shooterFeederMotor; private final RelativeEncoder gandalfEncoder; @@ -27,9 +30,24 @@ public void setShooterFeederVoltage(double voltage){ shooterFeederMotor.set(voltage); } + public double getGandalfEncoder() { + return gandalfEncoder.getVelocity(); + } + + public double getShooterFeederEncoder() { + return shooterFeederEncoder.getVelocity(); + } + public void stop(){ gandalfMotor.set(0); shooterFeederMotor.set(0); } + @Override + public void periodic() { + SmartDashboard.putNumber("Conveyor/Gandalf Motor Current", gandalfMotor.getOutputCurrent()); + SmartDashboard.putNumber("Conveyor/Shooter Feeder Motor Current", shooterFeederMotor.getOutputCurrent()); + SmartDashboard.putNumber("Conveyor/Gandalf Motor Speed", gandalfMotor.get()); + SmartDashboard.putNumber("Conveyor/Shooter Feeder Motor Spped", shooterFeederMotor.get()); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/IntakeTest.java b/src/main/java/com/stuypulse/robot/subsystems/IntakeTest.java index 352641a..060978e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/IntakeTest.java +++ b/src/main/java/com/stuypulse/robot/subsystems/IntakeTest.java @@ -4,7 +4,9 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.stuypulse.robot.constants.Ports; -public class IntakeTest { +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class IntakeTest extends SubsystemBase{ private final CANSparkMax motor; public IntakeTest() { @@ -16,6 +18,6 @@ public void setIntakeVoltage(double voltage) { public void stop() { motor.set(0); - } + } +} -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/ShooterTest.java b/src/main/java/com/stuypulse/robot/subsystems/ShooterTest.java index 87b0984..3071e88 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/ShooterTest.java +++ b/src/main/java/com/stuypulse/robot/subsystems/ShooterTest.java @@ -1,12 +1,15 @@ package com.stuypulse.robot.subsystems; import com.stuypulse.robot.constants.Ports; -import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; -public class ShooterTest { +public class ShooterTest extends SubsystemBase{ private final CANSparkMax leftMotor; private final CANSparkMax rightMotor; @@ -15,6 +18,9 @@ public class ShooterTest { public ShooterTest() { leftMotor = new CANSparkMax(Ports.Shooter.LEFT_MOTOR, MotorType.kBrushless); rightMotor = new CANSparkMax(Ports.Shooter.RIGHT_MOTOR, MotorType.kBrushless); + + leftEncoder = leftMotor.getEncoder(); + rightEncoder = rightMotor.getEncoder(); } public void setRightVoltage(double voltage) { @@ -29,4 +35,19 @@ public void stop() { leftMotor.set(0); rightMotor.set(0); } + + public double getRightShooterRPM(){ + return rightEncoder.getVelocity(); + } + public double getLeftShooterRPM(){ + return leftEncoder.getVelocity(); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Shooter/Right RPM", getRightShooterRPM()); + SmartDashboard.putNumber("Shooter/Left RPM", getLeftShooterRPM()); + SmartDashboard.putNumber("Shooter/Left Voltage", leftMotor.getBusVoltage() * leftMotor.getAppliedOutput()); + SmartDashboard.putNumber("Shooter/Right Voltage", rightMotor.getBusVoltage() * rightMotor.getAppliedOutput()); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/SwerveTest.java b/src/main/java/com/stuypulse/robot/subsystems/SwerveTest.java deleted file mode 100644 index 2ae6df0..0000000 --- a/src/main/java/com/stuypulse/robot/subsystems/SwerveTest.java +++ /dev/null @@ -1,5 +0,0 @@ -package com.stuypulse.robot.subsystems; - -public class SwerveTest { -//TODO modules then uh absolute encoders to test zero angles then ummmmmmm set voltage ^w^ -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swervetests/SwerveTest.java b/src/main/java/com/stuypulse/robot/subsystems/swervetests/SwerveTest.java new file mode 100644 index 0000000..485e039 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/swervetests/SwerveTest.java @@ -0,0 +1,43 @@ +package com.stuypulse.robot.subsystems.swervetests; + +import com.stuypulse.robot.subsystems.swervetests.module.SwerveModuleTest; +import com.stuypulse.robot.constants.Ports.Swerve.FrontRight; +import com.stuypulse.robot.subsystems.swervetests.module.SwerveModule; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SwerveTest extends SubsystemBase{ +private final SwerveModuleTest[] modules; + +SendableChooser moduleChooser = new SendableChooser(); +public SwerveTest(SwerveModuleTest... modules) { + + new SwerveModuleTest("FrontRight", 10, 11, 1); + new SwerveModuleTest("FrontLeft", 12, 13, 2); + new SwerveModuleTest("BackLeft", 14, 15, 3); + new SwerveModuleTest("BackRight", 16, 17, 4); + + this.modules = modules; + + for (var module : modules){ + moduleChooser.addOption(module.getID(), module); + } + + SmartDashboard.putData(moduleChooser); + } + + public void setDriveVoltage(double voltage){ + moduleChooser.getSelected().setDriveVoltage(voltage); + } + public void setTurnVoltage(double voltage){ + moduleChooser.getSelected().setTurnMotor(voltage); + } + + @Override + public void periodic(){ + SmartDashboard.putNumber("Turn Encoder Raw Vel", moduleChooser.getSelected().getTurnRawVelocity()); + SmartDashboard.putNumber("Drive Encoder RPM", moduleChooser.getSelected().getDriveEncoderRPM()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swervetests/module/SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swervetests/module/SwerveModule.java new file mode 100644 index 0000000..ed3ea0e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/swervetests/module/SwerveModule.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.subsystems.swervetests.module; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class SwerveModule extends SubsystemBase{ + public abstract String getID(); + + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swervetests/module/SwerveModuleTest.java b/src/main/java/com/stuypulse/robot/subsystems/swervetests/module/SwerveModuleTest.java new file mode 100644 index 0000000..5b5f04a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/swervetests/module/SwerveModuleTest.java @@ -0,0 +1,48 @@ +package com.stuypulse.robot.subsystems.swervetests.module; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; + + +public class SwerveModuleTest extends SwerveModule{ + private final CANSparkMax turnMotor; + private final CANSparkMax driveMotor; + private final String id; + + private final RelativeEncoder driveEncoder; + private final CANcoder turnEncoder; + + public SwerveModuleTest(String id, int driveID, int turnID, int encoderID) { + turnMotor = new CANSparkMax(turnID, MotorType.kBrushless); + driveMotor = new CANSparkMax(driveID, MotorType.kBrushless); + + driveEncoder = driveMotor.getEncoder(); + turnEncoder = new CANcoder(encoderID); + + this.id = id; + } + + public void setTurnMotor(double voltage){ + turnMotor.set(voltage); + } + + public void setDriveVoltage(double voltage) { + driveMotor.set(voltage); + } + + public double getDriveEncoderRPM() { + return driveEncoder.getVelocity(); + } + + public double getTurnRawVelocity() { + return turnEncoder.getVelocity().getValueAsDouble(); + } + + @Override + public String getID(){ + return id; + } +} \ No newline at end of file