Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

locked motors + shoot kinda works

Browse Source
This commit is contained in:
aarav18
2022-03-19 19:58:21 -06:00
parent dbc1e070e0
commit be850585ec
7 changed files with 87 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -277,7 +277,7 @@ public final class Constants {
public static final double TURRET_CLIMBING_POS = -3.76;
// Shoot Command Constants
public static final Gains SHOOT_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOT_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
/* Turret Constants */
// ID
src/main/java/frc4388/robot/Robot.java
+4 -4
View File
@@ -142,9 +142,9 @@ public class Robot extends TimedRobot {
m_robotTime.updateTimes();
// SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition());
// SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition());
current =
// current =
// m_robotContainer.m_robotBoomBoom.getCurrent() +
m_robotContainer.m_robotClimber.getCurrent(); //+
// m_robotContainer.m_robotClimber.getCurrent(); //+
// m_robotContainer.m_robotHood.getCurrent() +
// m_robotContainer.m_robotIntake.getCurrent() +
// m_robotContainer.m_robotExtender.getCurrent() +
@@ -152,8 +152,8 @@ public class Robot extends TimedRobot {
// m_robotContainer.m_robotStorage.getCurrent() +
// m_robotContainer.m_robotSwerveDrive.getCurrent();
// m_robotContainer.m_robotTurret.getCurrent();
SmartDashboard.putNumber("Total Robot Current Draw", current);
SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage());
// SmartDashboard.putNumber("Total Robot Current Draw", current);
// SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage());
SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent());
// Runs the Scheduler. This is responsible for polling buttons, adding
// newly-scheduled
src/main/java/frc4388/robot/RobotContainer.java
+36 -29
View File
@@ -112,7 +112,7 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
// public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
@@ -172,9 +172,9 @@ public class RobotContainer {
/* Default Commands */
// IK command
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000,
getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000,
// getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
// Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand(
@@ -208,17 +208,17 @@ public class RobotContainer {
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
// Turret Manual
// m_robotTurret.setDefaultCommand(
// new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
// m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
m_robotTurret.setDefaultCommand(
new RunCommand(() -> {
if (this.currentControlMode.equals(ControlMode.SHOOTER)) {
if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
}
if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
}, m_robotTurret));
new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
// m_robotTurret.setDefaultCommand(
// new RunCommand(() -> {
// if (this.currentControlMode.equals(ControlMode.SHOOTER)) {
// if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
// }
// if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
// }, m_robotTurret));
m_robotHood.setDefaultCommand(
new RunCommand(() -> {
@@ -226,12 +226,12 @@ public class RobotContainer {
if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
}, m_robotHood));
m_robotClimber.setDefaultCommand(
new RunCommand(() -> {
if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); }
if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000,
getOperatorController().getRightY() * 10000); }}
, m_robotClimber));
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> {
// if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); }
// if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000,
// getOperatorController().getRightY() * 10000); }}
// , m_robotClimber));
// m_robotTurret.setDefaultCommand(
// new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
@@ -269,15 +269,22 @@ public class RobotContainer {
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true))
.whenReleased(() -> m_robotSwerveDrive.stopModules());
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
.whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
.whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-45), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
.whileHeld(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret))
.whenReleased(() -> m_robotSwerveDrive.stopModules());
new JoystickButton(getDriverController(), XboxController.Button.kX.value)
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom));
/* Operator Buttons */
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
@@ -335,18 +342,18 @@ public class RobotContainer {
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
.whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber))
// .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber))
// .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender));
// .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
src/main/java/frc4388/robot/RobotMap.java
+6 -6
View File
@@ -140,13 +140,13 @@ public class RobotMap {
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
NeutralMode mode = NeutralMode.Coast;
leftFrontSteerMotor.setNeutralMode(mode);
leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
leftFrontWheelMotor.setNeutralMode(mode);// Coast
rightFrontSteerMotor.setNeutralMode(mode);
rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
rightFrontWheelMotor.setNeutralMode(mode);// Coast
leftBackSteerMotor.setNeutralMode(mode);
leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
leftBackWheelMotor.setNeutralMode(mode);// Coast
rightBackSteerMotor.setNeutralMode(mode);
rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
rightBackWheelMotor.setNeutralMode(mode);// Coast
// current limits
@@ -196,8 +196,8 @@ public class RobotMap {
}
/* Climb Subsystem */
public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
// public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO
// public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
/* Hooks Subsystem */
// public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless);
src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+24 -14
View File
@@ -53,7 +53,7 @@ public class Shoot extends CommandBase {
private double initialSwerveRotation;
// testing
private boolean simMode = true;
private boolean simMode = false;
private DummySensor driveDummy;
private DummySensor turretDummy;
@@ -99,10 +99,10 @@ public class Shoot extends CommandBase {
*/
public void updateError() {
m_targetPoint = SwerveDriveConstants.HUB_POSE;
m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
// m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
error = (m_targetAngle - turretDummy.get() + 360) % 360;
// error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
// m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
// error = (m_targetAngle - turretDummy.get() + 360) % 360;
error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
isAimedInTolerance = (Math.abs(error) <= tolerance);
if (simMode) {
@@ -115,8 +115,11 @@ public class Shoot extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_odoX = 0;//m_swerve.getOdometry().getX();
m_odoY = -1;//m_swerve.getOdometry().getY();
m_turret.gotoMidpoint();
m_odoX = 0;//-m_swerve.getOdometry().getY();
m_odoY = -8;//-m_swerve.getOdometry().getX();
m_gyroAngle = m_swerve.getRegGyro().getDegrees();
initialSwerveRotation = m_gyroAngle;
@@ -125,14 +128,15 @@ public class Shoot extends CommandBase {
m_targetVel = m_boomBoom.getVelocity(m_distance);
m_targetHood = m_boomBoom.getHood(m_distance);
m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
// m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
// deadzone processing
if (AimToCenter.isDeadzone(m_targetAngle)) {}
// initial error
updateError();
System.out.println("Error: " + error);
// System.out.println("Error: " + error);
prevError = error;
}
/**
@@ -159,22 +163,28 @@ public class Shoot extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (simMode) {
System.out.println("Normalized Output: " + normOutput);
}
// custom pid
runPID();
// custom pid
if (simMode) {
driveDummy.apply(normOutput);
System.out.println("Drive Dummy: " + driveDummy.get());
}
runPID();
SmartDashboard.putNumber("Error", this.error);
SmartDashboard.putNumber("Shoot.java TargetAngle", this.m_targetAngle);
SmartDashboard.putNumber("Normalized Output", normOutput);
m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the
// entire swerve drive or its the line below
// m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
m_hood.runAngleAdjustPID(m_targetHood);
m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
// m_hood.runAngleAdjustPID(m_targetHood);
// m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
if (simMode) {
turretDummy.apply(normOutput);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+8
View File
@@ -127,6 +127,14 @@ public class SwerveDrive extends SubsystemBase {
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
chassisSpeeds);
if (ignoreAngles) {
SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
for (int i = 0; i < states.length; i ++) {
SwerveModuleState state = states[i];
lockedStates[i]= new SwerveModuleState(0, state.angle);
}
setModuleStates(lockedStates);
}
setModuleStates(states);
// SmartDashboard.putNumber("rot", rot);
// SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+8
View File
@@ -38,6 +38,9 @@ public class SwerveModule extends SubsystemBase {
public double m_currentPos;
public double m_lastPos;
public SwerveModuleState lastState = new SwerveModuleState();
public SwerveModuleState currentState;
/** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
this.driveMotor = driveMotor;
@@ -181,10 +184,15 @@ public class SwerveModule extends SubsystemBase {
@Override
public void periodic() {
currentState = this.getState();
Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
lastState = currentState;
}
public void reset() {