mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Merge branch 'master' into covid-shooter-revert
This commit is contained in:
Nirvan Bhalala
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -0,0 +1 @@
|
||||
[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":1.6396523867809052,"y":-2.313833150808477},"rotation":{"radians":-0.008849326516257987}},"curvature":0.0},{"time":0.2722820970374773,"velocity":0.7351616620011887,"acceleration":2.7000000000000006,"pose":{"translation":{"x":1.739734776700425,"y":-2.3146446112802495},"rotation":{"radians":-0.007149647015655322}},"curvature":0.01725893438810635},{"time":0.35451476526736425,"velocity":0.9571898662218835,"acceleration":2.7,"pose":{"translation":{"x":1.8093165329850407,"y":-2.31510673816049},"rotation":{"radians":-0.006214537421300872}},"curvature":0.010262108695263363},{"time":0.4360203613484434,"velocity":1.1772549756407973,"acceleration":2.700000000000001,"pose":{"translation":{"x":1.8962996475995293,"y":-2.315615003996086},"rotation":{"radians":-0.005532904856197329}},"curvature":0.005996263181397757},{"time":0.5169500582728787,"velocity":1.3957651573367724,"acceleration":2.7000000000000015,"pose":{"translation":{"x":2.000415072981678,"y":-2.3161634275928744},"rotation":{"radians":-0.005039840497117272}},"curvature":0.0037792530506272166},{"time":0.5960164852085316,"velocity":1.6092445100630355,"acceleration":2.6999999999999944,"pose":{"translation":{"x":2.1192113689257215,"y":-2.316738666286958},"rotation":{"radians":-0.0046665773718276736}},"curvature":0.0026525886672486274},{"time":0.634352557101396,"velocity":1.7127519041737693,"acceleration":2.699999999999999,"pose":{"translation":{"x":2.18288684602207,"y":-2.317030681391432},"rotation":{"radians":-0.004508805721396696}},"curvature":0.0023250237048777172},{"time":0.6716263517870102,"velocity":1.8133911498249278,"acceleration":2.700000000000002,"pose":{"translation":{"x":2.248602565810843,"y":-2.317322139445587},"rotation":{"radians":-0.004363819082991022}},"curvature":0.0021040995089735234},{"time":0.7076518105628782,"velocity":1.9106598885197714,"acceleration":2.08330049831677,"pose":{"translation":{"x":2.315682270679263,"y":-2.31761024782456},"rotation":{"radians":-0.004227674108169131}},"curvature":0.0019683194092841986},{"time":0.7424426834355309,"velocity":1.983139731312244,"acceleration":-2.699999999999999,"pose":{"translation":{"x":2.3834160278296714,"y":-2.317892151968046},"rotation":{"radians":-0.0040968776842529794}},"curvature":0.0019052546752226036},{"time":0.7773927718968935,"velocity":1.888774492466565,"acceleration":-2.7000000000000037,"pose":{"translation":{"x":2.4510773500054253,"y":-2.318165001739701},"rotation":{"radians":-0.0039681904771730085}},"curvature":0.001909770538576822},{"time":0.8137373594388484,"velocity":1.7906441061032867,"acceleration":-2.699999999999997,"pose":{"translation":{"x":2.517940316216782,"y":-2.318426017786541},"rotation":{"radians":-0.0038384445909229676}},"curvature":0.0019833496346190354},{"time":0.851300180838236,"velocity":1.6892244883249403,"acceleration":-2.6999999999999953,"pose":{"translation":{"x":2.583296692466793,"y":-2.3186725578983505},"rotation":{"radians":-0.003704364009272917}},"curvature":0.002134423240000436},{"time":0.8898901860409195,"velocity":1.585031474277695,"acceleration":-2.700000000000001,"pose":{"translation":{"x":2.6464730524771953,"y":-2.318902183367081},"rotation":{"radians":-0.003562375965213496}},"curvature":0.002379784285935144},{"time":0.9693308968593537,"velocity":1.3705415550679225,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":2.7638687816148764,"y":-2.319302351210369},"rotation":{"radians":-0.003237626203522703}},"curvature":0.0032804752312573255},{"time":1.0503847372304065,"velocity":1.1516961860660802,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":2.8660868353612363,"y":-2.3196136033526837},"rotation":{"radians":-0.0028212080559704344}},"curvature":0.005135608410691401},{"time":1.1316234025131842,"velocity":0.9323517898025805,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":2.950739195403628,"y":-2.3198305248667666},"rotation":{"radians":-0.002252114531954038}},"curvature":0.008854161979101095},{"time":1.2129544945128348,"velocity":0.7127578414035238,"acceleration":-2.7,"pose":{"translation":{"x":3.0176383566138476,"y":-2.319957327362707},"rotation":{"radians":-0.001466708977573865}},"curvature":0.015315329026373479},{"time":1.4769388802178436,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.111716807318168,"y":-2.320018295432578},"rotation":{"radians":2.803592695601656E-14}},"curvature":1.0107951236368945E-13}]
|
||||
@@ -0,0 +1 @@
|
||||
[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":2.957088191715518,"y":-2.3571291631772207},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26544461524158175,"velocity":0.7167004611522707,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.0522103307783257,"y":-2.3571275191873893},"rotation":{"radians":5.101157578230443E-5}},"curvature":0.001038792914185728},{"time":0.3746678834467968,"velocity":1.0116032853063515,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.146595821982689,"y":-2.357116740977926},"rotation":{"radians":1.9099520049927404E-4}},"curvature":0.0019054639517066963},{"time":0.4574501551922932,"velocity":1.2351154190191918,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2395900571900658,"y":-2.357089632328051},"rotation":{"radians":4.035864972296716E-4}},"curvature":0.002652082514494386},{"time":0.5260164380469629,"velocity":1.4202443827267999,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.3306241198087982,"y":-2.357040972350315},"rotation":{"radians":6.754485548111559E-4}},"curvature":0.0033104753166963616},{"time":0.5850751482498664,"velocity":1.5797029002746394,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.419210597831801,"y":-2.3569673627586405},"rotation":{"radians":9.950959992412528E-4}},"curvature":0.0038981243771801294},{"time":0.6370367231616594,"velocity":1.7199991525364804,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.5049393968742493,"y":-2.356867075136363},"rotation":{"radians":0.0013520244174984744}},"curvature":0.0044210302444997965},{"time":0.6833390603048902,"velocity":1.8450154628232036,"acceleration":2.7,"pose":{"translation":{"x":3.587473553211266,"y":-2.3567398982042733},"rotation":{"radians":0.0017360184754233884}},"curvature":0.004874609692377478},{"time":0.7249302432685079,"velocity":1.957311656824971,"acceleration":2.6999999999999926,"pose":{"translation":{"x":3.6665450468156116,"y":-2.3565869850886587},"rotation":{"radians":0.002136559645089136}},"curvature":0.005243133825564394},{"time":0.7624827776596516,"velocity":2.0587034996810587,"acceleration":1.0819585313430768,"pose":{"translation":{"x":3.7419506143953716,"y":-2.3564107005893433},"rotation":{"radians":0.0025422828689825997}},"curvature":0.0054979122679133},{"time":0.7969484484947217,"velocity":2.095993926279525,"acceleration":-2.699999999999992,"pose":{"translation":{"x":3.813547562431645,"y":-2.3562144684477317},"rotation":{"radians":0.0029404502277413256}},"curvature":0.005594270462982216},{"time":0.8299507902622387,"velocity":2.0068876035072294,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":3.881249580216232,"y":-2.3560026186148484},"rotation":{"radians":0.003316424361517827}},"curvature":0.005467307107409776},{"time":0.8624379985890592,"velocity":1.919172141024814,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":3.9450225528893235,"y":-2.3557802345193797},"rotation":{"radians":0.00365313891813889}},"curvature":0.005026461101145579},{"time":0.9256149162430781,"velocity":1.748594463358963,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":4.06088076092986,"y":-2.355327048251995},"rotation":{"radians":0.004125323787162938}},"curvature":0.0026737820186648843},{"time":0.9861184332845145,"velocity":1.5852349673470847,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":4.161734080074728,"y":-2.354904842813889},"rotation":{"radians":0.004154108750296397}},"curvature":-0.0029414739347258},{"time":1.0438566564483853,"velocity":1.4293417648046338,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":4.24876157183169,"y":-2.3545658321706227},"rotation":{"radians":0.0034781161359367808}},"curvature":-0.01397812424405661},{"time":1.099078553226315,"velocity":1.2802426435042238,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":4.323575482900814,"y":-2.35435962366233},"rotation":{"radians":0.0018016415428211323}},"curvature":-0.032741911322928474},{"time":1.2053412709015958,"velocity":0.9933333057809655,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":4.444373578973541,"y":-2.354501684748035},"rotation":{"radians":-0.005323585518118219}},"curvature":-0.0899279066882791},{"time":1.3201225946749353,"velocity":0.6834237315929491,"acceleration":-2.7,"pose":{"translation":{"x":4.5405986995686,"y":-2.3554900302219766},"rotation":{"radians":-0.015505320196648706}},"curvature":-0.10506063032999254},{"time":1.5732424952649164,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":4.627077240224189,"y":-2.3571291631772207},"rotation":{"radians":-0.02086328407244092}},"curvature":2.0691998138625314E-15}]
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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()))
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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++;
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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(){
|
||||
|
||||
Reference in New Issue
Block a user