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

Merge branch 'master' into covid-shooter-revert

Browse Source
This commit is contained in:
Nirvan Bhalala
2021-04-22 16:42:19 -06:00
committed by GitHub
parent 2aa1aa8d6c 08963d3ee7
commit 785f984d9c
53 changed files with 484 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -36,7 +36,7 @@ public final class Constants {
public static final boolean isRightMotorInverted = true;
public static final boolean isLeftMotorInverted = false;
public static final boolean isRightArcadeInverted = false;
public static final boolean isAuxPIDInverted = true;
public static final boolean isAuxPIDInverted = false;
/* Drive Configuration */
public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config
src/main/java/frc4388/robot/Robot.java
+11 -3
View File
@@ -26,6 +26,9 @@ public class Robot extends TimedRobot {
Command m_autonomousCommand;
private RobotContainer m_robotContainer;
double m_initialTime;
double m_currentTime;
double m_deltaTime;
/**
* This function is run when the robot is first started up and should be
@@ -67,10 +70,12 @@ public class Robot extends TimedRobot {
/* Builds Autos */
m_robotContainer.buildAutos();
SmartDashboard.putString("Is Auto Start?", "NAH");
m_robotContainer.m_robotLime.limeOff();
}
@Override
public void disabledPeriodic() {
m_robotContainer.resetOdometry(new Pose2d());
}
/**
@@ -78,12 +83,12 @@ public class Robot extends TimedRobot {
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
m_robotContainer.setDriveGearState(true);
m_robotContainer.resetOdometry(new Pose2d());
m_initialTime = System.currentTimeMillis();
//m_robotContainer.resetGyroYawRobotContainer(0);
@@ -108,6 +113,9 @@ public class Robot extends TimedRobot {
*/
@Override
public void autonomousPeriodic() {
m_currentTime = System.currentTimeMillis();
m_deltaTime = m_currentTime - m_initialTime;
SmartDashboard.putNumber("Auto Path Time", (m_deltaTime)/1000);
}
@Override
src/main/java/frc4388/robot/RobotContainer.java
+94 -13
View File
@@ -38,14 +38,22 @@ import frc4388.robot.commands.auto.DriveOffLineBackward;
import frc4388.robot.commands.auto.DriveOffLineForward;
import frc4388.robot.commands.auto.EightBallAutoMiddle;
import frc4388.robot.commands.auto.FiveBallAutoMiddle;
import frc4388.robot.commands.auto.SequentialTest;
import frc4388.robot.commands.auto.SixBallAutoMiddle;
import frc4388.robot.commands.auto.Slalom;
import frc4388.robot.commands.auto.TankDriveVelocity;
import frc4388.robot.commands.auto.TenBallAutoMiddle;
import frc4388.robot.commands.InterruptSubystem;
import frc4388.robot.commands.auto.AutoPath1FromCenter;
import frc4388.robot.commands.auto.Barrel;
import frc4388.robot.commands.auto.BarrelMany;
import frc4388.robot.commands.auto.BarrelStart;
import frc4388.robot.commands.auto.Bounce;
import frc4388.robot.commands.auto.Wait;
import frc4388.robot.commands.climber.DisengageRachet;
import frc4388.robot.commands.climber.RunClimberWithTriggers;
import frc4388.robot.commands.climber.RunLevelerWithJoystick;
import frc4388.robot.commands.drive.DriveStraightAtVelocityPID;
import frc4388.robot.commands.drive.DriveStraightToPositionMM;
import frc4388.robot.commands.drive.DriveWithJoystick;
import frc4388.robot.commands.drive.PlaySongDrive;
@@ -106,7 +114,7 @@ public class RobotContainer {
/* Cameras */
private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
private final LimeLight m_robotLime = new LimeLight();
public final LimeLight m_robotLime = new LimeLight();
/* Controllers */
private static XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
@@ -119,6 +127,10 @@ public class RobotContainer {
SixBallAutoMiddle m_sixBallAutoMiddle;
SixBallAutoMiddle m_sixBallAutoMiddle0;
SixBallAutoMiddle m_sixBallAutoMiddle1;
EightBallAutoMiddle m_eightBallAutoMiddle;
DriveOffLineForward m_driveOffLineForward;
@@ -129,8 +141,21 @@ public class RobotContainer {
TenBallAutoMiddle m_tenBallAutoMiddle;
Slalom m_slalom;
Barrel m_barrel;
BarrelStart m_barrelStart;
BarrelMany m_barrelMany;
Bounce m_bounce;
SequentialTest m_sequentialTest;
public static boolean m_isShooterManual = false;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
@@ -171,9 +196,8 @@ public class RobotContainer {
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// runs the storage not
//m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage));
//m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
}
/**
@@ -190,12 +214,14 @@ public class RobotContainer {
.whenReleased(new InterruptSubystem(m_robotShooter))
.whenReleased(new InterruptSubystem(m_robotShooterHood));
// B driver test button
/*new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new TurnDegrees(m_robotDrive, 90));*/
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new Wait(m_robotDrive, 0, 0));
.whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive));
// X driver test button
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
@@ -326,6 +352,51 @@ public class RobotContainer {
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
String[] sixBallAutoMiddle0Paths = new String[]{
"SixBallMid0"
};
m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
String[] sixBallAutoMiddle1Paths = new String[]{
"SixBallMid1"
};
m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
String[] slalom = new String[]{
"Slalom"
};
m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
String[] barrel = new String[]{
"BarrelStart"
};
m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
String[] barrelStart = new String[]{
"Barrel"
};
m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
String[] bounce = new String[]{
"Bounce1",
"Bounce2",
"Bounce3",
"Bounce4"
};
m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce));
String[] barrelMany = new String[]{
"BarrelManyWaypoints"
};
m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
String[] eightBallAutoMiddlePaths = new String[]{
"EightBallMidComplete"
};
@@ -336,7 +407,7 @@ public class RobotContainer {
"DriveOffLineForward"
};
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths));
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
String[] driveOffLineBackwardPaths = new String[]{
"DriveOffLineBackward"
@@ -355,8 +426,14 @@ public class RobotContainer {
"TenBallMidComplete"
};
m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter,
m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths));
//m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths));
String[] sequentialTestPaths = new String[]{
"Seq1",
"Seq2"
};
m_sequentialTest = new SequentialTest(this, buildPaths(sequentialTestPaths));
}
/**
@@ -375,12 +452,15 @@ public class RobotContainer {
SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
//return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_driveOffLinfeBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_bounce.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
} catch (Exception e) {
System.err.println("ERROR");
}
@@ -506,6 +586,7 @@ public class RobotContainer {
return m_driverXbox;
}
/**
* Used for analog inputs like triggers and axises.
* @return The IHandController interface for the Operator Controller.
src/main/java/frc4388/robot/commands/auto/Barrel.java
+28
View File
@@ -0,0 +1,28 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class Barrel extends SequentialCommandGroup {
/**
* Creates a new Barrel.
*/
public Barrel(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/BarrelMany.java
+27
View File
@@ -0,0 +1,27 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class BarrelMany extends SequentialCommandGroup {
/**
* Creates a new BarrelMany.
*/
public BarrelMany(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
addCommands(
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/BarrelStart.java
+34
View File
@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class BarrelStart extends SequentialCommandGroup {
/**
* Creates a new BarrelStart.
*/
public BarrelStart(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0],
//new Wait(drive, 0.01, 1),
new TankDriveVelocity(drive, 5, 5, 1.2) //my life be like oooooo aaaaaa ooooo aaaa
);
}
}
src/main/java/frc4388/robot/commands/auto/Bounce.java
+43
View File
@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class Bounce extends SequentialCommandGroup {
/**
* Creates a new Bounce.
*/
public Bounce(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0],
new InstantCommand(() -> drive.switchReversed(true)),
new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
paths[1],
new InstantCommand(() -> drive.switchReversed(false)),
new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
paths[2],
new InstantCommand(() -> drive.switchReversed(true)),
new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
paths[3],
new InstantCommand(() -> drive.switchReversed(false)),
new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d()))
);
}
}
src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java
+6 -1
View File
@@ -7,8 +7,11 @@
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
@@ -18,11 +21,13 @@ public class DriveOffLineForward extends SequentialCommandGroup {
/**
* Creates a new DriveOffLineForward.
*/
public DriveOffLineForward(Drive drive, RamseteCommand[] paths) {
public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
new InstantCommand(() -> drive.switchReversed(true)),
new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
paths[0]
);
}
src/main/java/frc4388/robot/commands/auto/SequentialTest.java
+33
View File
@@ -0,0 +1,33 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class SequentialTest extends SequentialCommandGroup {
/**
* Creates a new SequentialTest.
*/
public SequentialTest(RobotContainer m_robotContainer, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0],
new InstantCommand(() -> m_robotContainer.resetOdometry(new Pose2d())),
paths[1]
);
}
}
src/main/java/frc4388/robot/commands/auto/Slalom.java
+28
View File
@@ -0,0 +1,28 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class Slalom extends SequentialCommandGroup {
/**
* Creates a new Slalom.
*/
public Slalom(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java
+11 -11
View File
@@ -15,10 +15,10 @@ public class TankDriveVelocity extends CommandBase {
double m_leftTargetVel;
double m_rightTargetVel;
double m_targetTime;
double m_firstTimeSec;
double m_currentTimeSec;
double m_diffSec;
long m_targetTime;
long m_firstTime;
long m_currentTime;
long m_diffTime;
/**
* Creates a new TankDriveVelocity.
@@ -28,24 +28,24 @@ public class TankDriveVelocity extends CommandBase {
m_drive = subsystem;
m_leftTargetVel = leftTargetVel;
m_rightTargetVel = rightTargetVel;
m_targetTime = targetTime;
m_targetTime = (long) (targetTime * 1000);
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_firstTimeSec = (System.currentTimeMillis() / 1000);
m_diffSec = 0;
m_firstTime = System.currentTimeMillis();
m_diffTime = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_currentTimeSec = (System.currentTimeMillis() / 1000);
m_diffSec = m_currentTimeSec - m_firstTimeSec;
m_currentTime = System.currentTimeMillis();
m_diffTime = m_currentTime - m_firstTime;
if (m_diffSec < m_targetTime) {
if (m_diffTime < m_targetTime) {
m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel);
}
@@ -59,7 +59,7 @@ public class TankDriveVelocity extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_diffSec >= m_targetTime) {
if (m_diffTime >= m_targetTime) {
return true;
}
return false;
src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java
+1 -1
View File
@@ -29,7 +29,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetGyro = (m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
}
// Called every time the scheduler runs while the command is scheduled.
src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java
+1 -1
View File
@@ -46,7 +46,7 @@ public class GotoCoordinatesRobotRelative extends SequentialCommandGroup {
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0, 0),
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
new TurnDegrees(m_drive, (m_endAngle - m_currentAngle) % 360));
}
public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
src/main/java/frc4388/robot/commands/drive/TurnDegrees.java
+4 -3
View File
@@ -38,6 +38,7 @@ public class TurnDegrees extends CommandBase {
public void initialize() {
m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
SmartDashboard.putNumber("Current Yaw Ticks", m_currentYawInTicks);
m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks;
i = 0;
@@ -48,10 +49,10 @@ public class TurnDegrees extends CommandBase {
public void execute() {
m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
m_drive.runTurningPID(m_targetAngleTicksOut);
m_drive.runTurningPID(m_targetAngleTicksIn);
//SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
//SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
i++;
}
src/main/java/frc4388/robot/subsystems/Drive.java
+21 -3
View File
@@ -46,6 +46,7 @@ public class Drive extends SubsystemBase {
public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
public static GyroBase m_pigeonGyro;
public boolean m_isReversed;
/* Drive objects to manage Drive Train */
public DifferentialDrive m_driveTrain;
@@ -339,9 +340,26 @@ public class Drive extends SubsystemBase {
m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
updateOdometry(m_isReversed);
}
public void updateOdometry(boolean reversed){
if (reversed){
m_odometry.update(Rotation2d.fromDegrees( -getHeading()-180),
-inchesToMeters(getDistanceInches(m_rightFrontMotor)),
inchesToMeters(getDistanceInches(m_leftFrontMotor)));
}
else
{
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_leftFrontMotor)),
-inchesToMeters(getDistanceInches(m_rightFrontMotor)));
}
}
public void switchReversed(boolean reversed)
{
m_isReversed = reversed;
}
/**
@@ -401,7 +419,7 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
@@ -426,7 +444,7 @@ public class Drive extends SubsystemBase {
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
}
/**
src/main/java/frc4388/robot/subsystems/LimeLight.java
+1 -1
View File
@@ -20,7 +20,7 @@ public class LimeLight extends SubsystemBase {
public void limeOff(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
}
public void limeOn(){