finished TestProject for testing motors on the robot. Done logging w/…

… encodrs and did sweeve. yay !!!!!! :3

Co-authored-by: naveed1117 <naveed1117@users.noreply.github.com>
Co-authored-by: jopy-man <jopy-man@users.noreply.github.com>
Co-authored-by: Lapcas <Lapcas@users.noreply.github.com>

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -46,7 +46,7 @@ private void configureDefaultCommands() {}
     private void configureButtonBindings() {}
 
     /**************/
-    /*** AUTONS ***/
+    /*** AUTONS ***/    
     /**************/
 
     public void configureAutons() {

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());
+    }
 }

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());
+    }
 }
 
 

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());
+    }
 }

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);
-    }
+    } 
+}
 
-}

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());
+    }
 }

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<SwerveModuleTest> moduleChooser = new SendableChooser<SwerveModuleTest>();
+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());
+    }
+}

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();
+
+
+}

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;
+    }
+}