From 747b4bf72f554ea7685ed7cf9380c2e37b323337 Mon Sep 17 00:00:00 2001 From: Aarav Date: Thu, 27 Apr 2023 17:55:04 -0600 Subject: [PATCH] drive base working now --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 3 ++- src/main/java/frc4388/robot/RobotMap.java | 16 ++++++++-------- .../java/frc4388/robot/subsystems/Drive.java | 15 +++++++++++++-- 4 files changed, 25 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 95faaf0..33a89a9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -12,8 +12,8 @@ package frc4388.robot; public final class Constants { public static final class DriveConstants { public static final int DRIVE_LEFT_LEADER_ID = 2; - public static final int DRIVE_RIGHT_LEADER_ID = 3; - public static final int DRIVE_LEFT_FOLLOWER_ID = 4; + public static final int DRIVE_RIGHT_LEADER_ID = 4; + public static final int DRIVE_LEFT_FOLLOWER_ID = 3; public static final int DRIVE_RIGHT_FOLLOWER_ID = 5; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b499a90..5547923 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,7 +29,7 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final Drive m_robotDrive = new Drive(m_robotMap.driveMotorLeftLeader, m_robotMap.driveMotorRightLeader, m_robotMap.driveBase); + private final Drive m_robotDrive = new Drive(m_robotMap.driveMotorLeftLeader, m_robotMap.driveMotorRightLeader, m_robotMap.driveMotorLeftFollower, m_robotMap.driveMotorRightFollower, m_robotMap.driveBase); private final Shooter m_robotShooterBottomLeft = new Shooter(m_robotMap.shooterSolenoidBottomLeft); private final Shooter m_robotShooterBottomMiddle = new Shooter(m_robotMap.shooterSolenoidBottomMiddle); @@ -63,6 +63,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller m_robotDrive.setDefaultCommand(new RunCommand(() -> m_robotDrive.arcadeDrive(getController().getLeftY(), getController().getRightX()), m_robotDrive)); + // m_robotDrive.setDefaultCommand(new RunCommand(() -> m_robotDrive.tankDrive(getController().getLeftY(), getController().getLeftY()), m_robotDrive)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 71a0fe6..d27b11e 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -49,20 +49,20 @@ public class RobotMap { driveMotorLeftFollower.setNeutralMode(NeutralMode.Brake); driveMotorRightFollower.setNeutralMode(NeutralMode.Brake); - driveMotorLeftFollower.follow(driveMotorLeftLeader); - driveMotorRightFollower.follow(driveMotorRightLeader); + // driveMotorLeftFollower.follow(driveMotorLeftLeader); + // driveMotorRightFollower.follow(driveMotorRightLeader); } /* Horn Subsystem */ //public final Solenoid hornSolenoid = new Solenoid(PneumaticsModuleType.REVPH, HORN_SOLENOID_ID); /* Shooter Subsystem */ - public final Solenoid shooterSolenoidBottomLeft = new Solenoid(PneumaticsModuleType.REVPH, SHOOTER_SOLENOID_BOTTOM_LEFT_ID); - public final Solenoid shooterSolenoidBottomMiddle = new Solenoid(PneumaticsModuleType.REVPH, SHOOTER_SOLENOID_BOTTOM_MIDDLE_ID); - public final Solenoid shooterSolenoidBottomRight = new Solenoid(PneumaticsModuleType.REVPH, SHOOTER_SOLENOID_BOTTOM_RIGHT_ID); - public final Solenoid shooterSolenoidTopLeft = new Solenoid(PneumaticsModuleType.REVPH, SHOOTER_SOLENOID_TOP_LEFT_ID); - public final Solenoid shooterSolenoidTopMiddle = new Solenoid(PneumaticsModuleType.REVPH, SHOOTER_SOLENOID_TOP_MIDDLE_ID); - public final Solenoid shooterSolenoidTopRight = new Solenoid(PneumaticsModuleType.REVPH, SHOOTER_SOLENOID_TOP_RIGHT_ID); + public final Solenoid shooterSolenoidBottomLeft = new Solenoid(PneumaticsModuleType.CTREPCM, SHOOTER_SOLENOID_BOTTOM_LEFT_ID); + public final Solenoid shooterSolenoidBottomMiddle = new Solenoid(PneumaticsModuleType.CTREPCM, SHOOTER_SOLENOID_BOTTOM_MIDDLE_ID); + public final Solenoid shooterSolenoidBottomRight = new Solenoid(PneumaticsModuleType.CTREPCM, SHOOTER_SOLENOID_BOTTOM_RIGHT_ID); + public final Solenoid shooterSolenoidTopLeft = new Solenoid(PneumaticsModuleType.CTREPCM, SHOOTER_SOLENOID_TOP_LEFT_ID); + public final Solenoid shooterSolenoidTopMiddle = new Solenoid(PneumaticsModuleType.CTREPCM, SHOOTER_SOLENOID_TOP_MIDDLE_ID); + public final Solenoid shooterSolenoidTopRight = new Solenoid(PneumaticsModuleType.CTREPCM, SHOOTER_SOLENOID_TOP_RIGHT_ID); // public final Solenoid shooterSolenoidBottomLeftOuter = new Solenoid(PneumaticsModuleType.REVPH, SHOOTER_SOLENOID_BOTTOM_LEFT_OUTER_ID); // public final Solenoid shooterSolenoidBottomLeftInner = new Solenoid(PneumaticsModuleType.REVPH, SHOOTER_SOLENOID_BOTTOM_LEFT_INNER_ID); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 2496e2f..f28b7ed 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -2,22 +2,33 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Drive extends SubsystemBase { private final WPI_TalonSRX m_leftMotor; private final WPI_TalonSRX m_rightMotor; + private final WPI_TalonSRX m_leftFollowMotor; + private final WPI_TalonSRX m_rightFollowMotor; private final DifferentialDrive m_driveBase; - public Drive(WPI_TalonSRX leftMotor, WPI_TalonSRX rightMotor, DifferentialDrive driveBase) { + public Drive(WPI_TalonSRX leftMotor, WPI_TalonSRX rightMotor, WPI_TalonSRX leftFollowMotor, WPI_TalonSRX rightFollowMotor, DifferentialDrive driveBase) { m_leftMotor = leftMotor; m_rightMotor = rightMotor; + m_leftFollowMotor = leftFollowMotor; + m_rightFollowMotor = rightFollowMotor; m_driveBase = driveBase; } public void arcadeDrive(double xSpeed, double zRotation) { - m_driveBase.arcadeDrive(xSpeed, zRotation, true); + m_driveBase.arcadeDrive(xSpeed, zRotation); + + m_rightMotor.set(-m_rightMotor.get()); + + m_rightFollowMotor.follow(m_rightMotor); + m_leftFollowMotor.follow(m_leftMotor); + } public void tankDrive(double leftSpeed, double rightSpeed) {