drive base working now

This commit is contained in:
Aarav
2023-04-27 17:55:04 -06:00
parent 12b02a8c99
commit 747b4bf72f
4 changed files with 25 additions and 13 deletions
+2 -2
View File
@@ -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;
}
@@ -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));
}
/**
+8 -8
View File
@@ -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);
@@ -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) {