From 05900f8c523c39500da7960db3447e5c7d9182f4 Mon Sep 17 00:00:00 2001 From: team6101 Date: Tue, 5 Mar 2024 15:29:30 -0500 Subject: [PATCH] Fix motor IDs & settings --- src/main/java/frc/robot/Constants.java | 20 +++--- src/main/java/frc/robot/Robot.java | 65 ++++++++++++------- .../frc/robot/RobotComponents/Shooter.java | 4 +- src/main/java/frc/robot/auton/AutonShoot.java | 4 +- 4 files changed, 57 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5246417..414b3f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,29 +6,31 @@ public class Constants { public static final int DRIVE_LEFT_PARENT_ID = 4; public static final int DRIVE_LEFT_CHILD_ID = 8; public static final int DRIVE_RIGHT_PARENT_ID = 11; - public static final int DRIVE_RIGHT_CHILD_ID = 3; - public static final int SHOOTER_LEFT_FLYWHEEL_ID = 6; + public static final int DRIVE_RIGHT_CHILD_ID = 14; + public static final int SHOOTER_LEFT_FLYWHEEL_ID = 9; public static final int SHOOTER_RIGHT_FLYWHEEL_ID = 7; - public static final int SHOOTER_FEEDER_ID = 9; + public static final int SHOOTER_FEEDER_ID = 10; public static final int INTAKE_ID = 6101; /*CLIMB MOTOR IDS */ - public static final int LEFT_CLIMB_ID = 31; - public static final int RIGHT_CLIMB_ID = 21; + public static final int LEFT_CLIMB_ID = 6; + public static final int RIGHT_CLIMB_ID = 5; /*SHOOTER CONFIG*/ /**the amount of time we let the wheels rev up when shooting in seconds */ - public static final double SHOOTER_FLYWHEEL_STARTUP_TIME = 1.0; + public static final double SHOOTER_FLYWHEEL_STARTUP_TIME = 0.25; + public static final double SHOOTER_LEFT_FLYWHEEL_FLY_SPEED = 1.0; + public static final double SHOOTER_RIGHT_FLYWHEEL_FLY_SPEED = 1.0; /*The amount of seconds that the feeder should wait before turning off after shooting */ - public static final double FEEDER_MOTOR_SHUTOFF_TIME = 0; + public static final double FEEDER_MOTOR_SHUTOFF_TIME = 1.5; public static final double SHOOTING_VELOCITY_RPM = 4000; /*CLIMB CONFIG */ public static final double CLIMB_EXTENDED_ENCODER_POSITION = 5; public static final double CLIMB_RETRACTED_ENCODER_POSITION = 0.5; /**the % of max power we give to the arm motors while extending */ - public static final double CLIMB_EXTENSION_SPEED = .25; + public static final double CLIMB_EXTENSION_SPEED = .5; /**the % of max power we give to the arm motors while retracting */ public static final double CLIMB_RETRACTION_SPEED = -.25; @@ -68,7 +70,7 @@ public class Constants { public static final int PID_LOOP_IDX = 0; /** amount of time in ms to wait for confirmation */ public static final int TIMEOUT_MS = 30; - public static final double ENCODER_ROTATION = 4096.0; + public static final int ENCODER_ROTATION = 4096; /*OTHER CONFIG */ public static final double MOTOR_BACKTRACK_SPEED_PERCENT = -0.5; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a954f1e..059d4b5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,7 @@ import frc.robot.auton.SequentialActionRunner; import frc.robot.motor.MotorController; import frc.robot.motor.MotorControllerFactory; -import frc.robot.vision.AprilTagHighlighter; +// import frc.robot.vision.AprilTagHighlighter; import java.util.ArrayDeque; /** @@ -31,29 +31,20 @@ public class Robot extends TimedRobot { //hi // hello!!!!!!! - AprilTagHighlighter aprilTagHighlighter; + // AprilTagHighlighter aprilTagHighlighter; public static RobotMotors motors; - MotorController driveLeftParent = RobotConfigs.getLeftParent(); - MotorController driveLeftChild = RobotConfigs.getLeftChild(); - MotorController driveRightParent = RobotConfigs.getRightParent(); - MotorController driveRightChild = RobotConfigs.getRightChild(); - MotorController leftFlywheel = MotorControllerFactory.create( - Constants.SHOOTER_LEFT_FLYWHEEL_ID, - MotorController.Type.SparkMax - ); - MotorController rightFlywheel = MotorControllerFactory.create( - Constants.SHOOTER_RIGHT_FLYWHEEL_ID, - MotorController.Type.SparkMax - ); - MotorController feederMotor = MotorControllerFactory.create( - Constants.SHOOTER_FEEDER_ID, - MotorController.Type.SparkMax - ); - MotorController leftClimb = MotorControllerFactory.create(Constants.LEFT_CLIMB_ID, MotorController.Type.Talon); - MotorController rightClimb = MotorControllerFactory.create(Constants.RIGHT_CLIMB_ID, MotorController.Type.Talon); - MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMax); + MotorController driveLeftParent; + MotorController driveLeftChild; + MotorController driveRightParent; + MotorController driveRightChild; + MotorController leftFlywheel; + MotorController rightFlywheel; + MotorController feederMotor; + MotorController leftClimb; + MotorController rightClimb; + // MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMaxBrushless); XboxController driverController = new XboxController(Constants.DRIVER_CONTROLLER_ID); XboxController coDriverController = new XboxController(Constants.CODRIVER_CONTROLLER_ID); static AHRS navX = new AHRS(SPI.Port.kMXP); @@ -76,6 +67,23 @@ public static AHRS getGyroscope() { */ @Override public void robotInit() { + driveLeftParent = + MotorControllerFactory.create(Constants.DRIVE_LEFT_PARENT_ID, MotorController.Type.SparkMaxBrushless); + driveLeftChild = + MotorControllerFactory.create(Constants.DRIVE_LEFT_CHILD_ID, MotorController.Type.SparkMaxBrushless); + driveRightParent = + MotorControllerFactory.create(Constants.DRIVE_RIGHT_PARENT_ID, MotorController.Type.SparkMaxBrushless); + driveRightChild = + MotorControllerFactory.create(Constants.DRIVE_RIGHT_CHILD_ID, MotorController.Type.SparkMaxBrushless); + leftFlywheel = + MotorControllerFactory.create(Constants.SHOOTER_LEFT_FLYWHEEL_ID, MotorController.Type.SparkMaxBrushless); + rightFlywheel = + MotorControllerFactory.create(Constants.SHOOTER_RIGHT_FLYWHEEL_ID, MotorController.Type.SparkMaxBrushless); + feederMotor = + MotorControllerFactory.create(Constants.SHOOTER_FEEDER_ID, MotorController.Type.SparkMaxBrushless); + leftClimb = MotorControllerFactory.create(Constants.LEFT_CLIMB_ID, MotorController.Type.SparkMaxBrushed); + rightClimb = MotorControllerFactory.create(Constants.RIGHT_CLIMB_ID, MotorController.Type.SparkMaxBrushed); + initializeSmartMotion(driveLeftParent, Constants.NORMAL_ROBOT_GAINS); initializeSmartMotion(driveRightParent, Constants.NORMAL_ROBOT_GAINS); @@ -91,15 +99,26 @@ public void robotInit() { driveRightParent.setInverted(false); feederMotor.setInverted(true); + //This false is required feederMotor.setBrakeMode(false); + rightFlywheel.setInverted(true); + leftFlywheel.setInverted(false); + System.out.println("Is drive right parent inverted? " + driveRightParent.getInverted()); + System.out.println( + "Do we have cool navX? " + getGyroscope().isAltitudeValid() + " temp: " + getGyroscope().getTempC() + ); + driveLeftChild.setBrakeMode(false); driveLeftParent.setBrakeMode(false); driveRightChild.setBrakeMode(false); driveRightParent.setBrakeMode(false); + leftClimb.setBrakeMode(true); + rightClimb.setBrakeMode(true); + motors = new RobotMotors() .driveLeftParent(driveLeftParent) @@ -110,8 +129,8 @@ public void robotInit() { .leftFlywheel(leftFlywheel) .rightFlywheel(rightFlywheel) .leftClimb(leftClimb) - .rightClimb(rightClimb) - .intake(intake); + .rightClimb(rightClimb); + // .intake(intake); getGyroscope().reset(); System.out.println(Constants.APRIL_TAG_CONFIDENCE_FRAMES); SmartDashboard.putNumber("rotationGainsP", Constants.ROTATION_GAINS.P); diff --git a/src/main/java/frc/robot/RobotComponents/Shooter.java b/src/main/java/frc/robot/RobotComponents/Shooter.java index 9d8162b..905a9d6 100644 --- a/src/main/java/frc/robot/RobotComponents/Shooter.java +++ b/src/main/java/frc/robot/RobotComponents/Shooter.java @@ -8,8 +8,8 @@ public class Shooter { public static void startShooterMotors() { - Robot.motors.getLeftFlywheel().setVelocity(Constants.SHOOTING_VELOCITY_RPM); - Robot.motors.getRightFlywheel().setVelocity(Constants.SHOOTING_VELOCITY_RPM); + Robot.motors.getLeftFlywheel().set(Constants.SHOOTER_LEFT_FLYWHEEL_FLY_SPEED); + Robot.motors.getRightFlywheel().set(Constants.SHOOTER_RIGHT_FLYWHEEL_FLY_SPEED); } public static void stopShooterMotors() { diff --git a/src/main/java/frc/robot/auton/AutonShoot.java b/src/main/java/frc/robot/auton/AutonShoot.java index 0af18ed..b264ca3 100644 --- a/src/main/java/frc/robot/auton/AutonShoot.java +++ b/src/main/java/frc/robot/auton/AutonShoot.java @@ -12,7 +12,7 @@ public class AutonShoot extends AutonAction { public boolean isDone() { if (Timer.getFPGATimestamp() >= revFinishedTime) { Robot.motors.getFeeder().set(0.2); - Robot.motors.getIntake().set(.2); + //Robot.motors.getIntake().set(.2); } return Timer.getFPGATimestamp() >= revFinishedTime + 1.0; @@ -21,7 +21,7 @@ public boolean isDone() { @Override public void initiate() { Robot.motors.getLeftFlywheel().set(1.0); - Robot.motors.getRightFlywheel().set(-1.0); + Robot.motors.getRightFlywheel().set(1.0); revFinishedTime = Timer.getFPGATimestamp() + Constants.SHOOTER_FLYWHEEL_STARTUP_TIME; }