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

did a lot

Browse Source
This commit is contained in:
Aarav Shah
2020-03-10 20:13:52 -06:00
parent 28d9e0a5f4
commit 59fa78f6e3
18 changed files with 177 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
PathWeaver/Paths/DriveOffLineBackward
+3
View File
@@ -0,0 +1,3 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
3.2,-2.3,0.5,0.0,true,
3.7,-2.3,0.5,0.0,true,
PathWeaver/Paths/DriveOffLineForward
+3
View File
@@ -0,0 +1,3 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
3.2,-2.3,0.5,0.0,true,
3.7,-2.3,0.5,0.0,true,
PathWeaver/Paths/EightBallMid2
+1 -1
View File
@@ -1,3 +1,3 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
7.2,-0.715,1.5,0.0,true,
6.8,-2.6,-0.6,-0.3,true,
6.612,-2.7,-0.6,-0.3,true,
PathWeaver/Paths/EightBallMidComplete
+2 -2
View File
@@ -2,5 +2,5 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name
3.2,-2.4,0.2,2.5,true,
5.0,-1.1,3.0,0.0,true,
7.2,-1.1,1.5,0.0,true,
6.5,-2.78,-0.39262822032017564,-0.17846737287280634,true,
6.291072284729572,-2.036118403443617,0.13087607344005825,0.40452604517836144,true,
6.6123135559006245,-2.7023965955020937,-0.39262822032017564,-0.17846737287280634,true,
6.279174459871384,-2.2621770757491713,0.13087607344005825,0.40452604517836144,true,
PathWeaver/pathweaver.json
+2 -2
View File
@@ -1,8 +1,8 @@
{
"lengthUnit": "Meter",
"exportUnit": "Always Meters",
"maxVelocity": 2.5,
"maxAcceleration": 3.0,
"maxVelocity": 1.7,
"maxAcceleration": 2.7,
"wheelBase": 0.648,
"gameName": "Infinite Recharge",
"outputDir": ".."
src/main/deploy/paths/DriveOffLineBackward.wpilib.json
+1
View File
@@ -0,0 +1 @@
[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]
src/main/deploy/paths/DriveOffLineForward.wpilib.json
+1
View File
@@ -0,0 +1 @@
[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]
src/main/deploy/paths/EightBallMid0.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/EightBallMid1.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/EightBallMid2.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/EightBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/SixBallMid0.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/SixBallMid1.wpilib.json
+1 -1
View File
@@ -1 +1 @@
[{"time":0.0,"velocity":0.0,"acceleration":3.0,"pose":{"translation":{"x":5.006,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.2519761297876903,"velocity":0.7559283893630709,"acceleration":2.9999999999999987,"pose":{"translation":{"x":5.101237954974175,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3562702409671562,"velocity":1.0688107229014685,"acceleration":3.0000000000000013,"pose":{"translation":{"x":5.1963927268981935,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.4361595761278285,"velocity":1.3084787283834856,"acceleration":3.0,"pose":{"translation":{"x":5.291352763772011,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.503308309644587,"velocity":1.5099249289337608,"acceleration":3.000000000000002,"pose":{"translation":{"x":5.385978881835937,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.562202759020208,"velocity":1.686608277060624,"acceleration":3.000000000000002,"pose":{"translation":{"x":5.480107913374901,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6151186089000533,"velocity":1.8453558267001602,"acceleration":3.0,"pose":{"translation":{"x":5.573556354522705,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6633872740545509,"velocity":1.990161822163653,"acceleration":3.0,"pose":{"translation":{"x":5.666124013066292,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7078595229045569,"velocity":2.123578568713671,"acceleration":3.000000000000005,"pose":{"translation":{"x":5.757597656250001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7491126566722462,"velocity":2.2473379700167393,"acceleration":3.000000000000005,"pose":{"translation":{"x":5.847754658579826,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7875559872701902,"velocity":2.3626679618105713,"acceleration":3.0,"pose":{"translation":{"x":5.936366649627686,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.823489794243851,"velocity":2.470469382731554,"acceleration":0.8651265291285504,"pose":{"translation":{"x":6.023203161835671,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8576242431067156,"velocity":2.5,"acceleration":-1.1732826408603372,"pose":{"translation":{"x":6.108035278320313,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8909260810512718,"velocity":2.460927531630908,"acceleration":-2.9999999999999947,"pose":{"translation":{"x":6.190639280676843,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9241733364212149,"velocity":2.361185765521079,"acceleration":-3.0,"pose":{"translation":{"x":6.270800296783448,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9577172640528673,"velocity":2.2605539826261216,"acceleration":-3.0,"pose":{"translation":{"x":6.3483159486055385,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9915130896437078,"velocity":2.1591665058536003,"acceleration":-3.000000000000006,"pose":{"translation":{"x":6.423000000000001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.025517145389847,"velocity":2.0571543386151823,"acceleration":-3.0000000000000098,"pose":{"translation":{"x":6.494686004519464,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0596888686194887,"velocity":1.9546391689262566,"acceleration":-3.0000000000000036,"pose":{"translation":{"x":6.563230953216554,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0939935025170637,"velocity":1.8517252672335311,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.628518922448159,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1284057930259193,"velocity":1.7484883957069648,"acceleration":-3.0,"pose":{"translation":{"x":6.690464721679689,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1975327505604134,"velocity":1.5411075231034823,"acceleration":-2.999999999999999,"pose":{"translation":{"x":6.804164600372316,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.267315103090996,"velocity":1.3317604655117345,"acceleration":-2.9999999999999987,"pose":{"translation":{"x":6.904402343750003,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3388330522864487,"velocity":1.1172066179253766,"acceleration":-3.0,"pose":{"translation":{"x":6.9919748954772984,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4151734608431514,"velocity":0.8881853922552679,"acceleration":-3.0,"pose":{"translation":{"x":7.0685211181640675,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.505709141675879,"velocity":0.6165783497570856,"acceleration":-3.0,"pose":{"translation":{"x":7.136638523101813,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7112352582615742,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":7.200000000000008,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0}]
[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":5.006,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.2656061620411004,"velocity":0.717136637510971,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.101237954974175,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3755418079977478,"velocity":1.013962881593919,"acceleration":2.699999999999999,"pose":{"translation":{"x":5.1963927268981935,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45975256128584724,"velocity":1.2413319154717875,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.291352763772011,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5305335412553954,"velocity":1.4324405613895677,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.385978881835937,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5926137417782101,"velocity":1.6000571028011674,"acceleration":1.7647018169830884,"pose":{"translation":{"x":5.480107913374901,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6492481806601815,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.573556354522705,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7036997445093501,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.666124013066292,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7575077699115317,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.757597656250001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.810541300693782,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.847754658579826,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8626660013101702,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.936366649627686,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9137463026089846,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.023203161835671,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9636475475999504,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.108035278320313,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0122381372214386,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.190639280676843,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0593916761076771,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.270800296783448,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1049891183559657,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.3483159486055385,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1489209132938847,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.423000000000001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1910891512465098,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.494686004519464,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2314097093036216,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.563230953216554,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2698143970869191,"velocity":1.7,"acceleration":-1.1179894261480032,"pose":{"translation":{"x":6.628518922448159,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3067004902763237,"velocity":1.6587617378423356,"acceleration":-2.700000000000003,"pose":{"translation":{"x":6.690464721679689,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3795667014519033,"velocity":1.4620229676682703,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.804164600372316,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4531237596123885,"velocity":1.26341891063496,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.904402343750003,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.528510297293001,"velocity":1.0598752588973066,"acceleration":-2.7,"pose":{"translation":{"x":6.9919748954772984,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6089801534753305,"velocity":0.8426066472050169,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.0685211181640675,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7044131404571534,"velocity":0.5849375823540952,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.136638523101813,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9210566894771886,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.200000000000008,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0}]
src/main/deploy/paths/SixBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/java/frc4388/robot/RobotContainer.java
+30 -5
View File
@@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
@@ -33,6 +34,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.commands.auto.DriveOffLineBackward;
import frc4388.robot.commands.auto.DriveOffLineForward;
import frc4388.robot.commands.auto.EightBallAutoMiddle;
import frc4388.robot.commands.auto.SixBallAutoMiddle;
import frc4388.robot.commands.auto.Wait;
@@ -92,11 +95,16 @@ public class RobotContainer {
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
/* Autos */
SixBallAutoMiddle m_sixBallAutoMiddle;
double m_totalTimeAuto;
SixBallAutoMiddle m_sixBallAutoMiddle;
EightBallAutoMiddle m_eightBallAutoMiddle;
DriveOffLineForward m_driveOffLineForward;
DriveOffLineBackward m_driveOffLineBackward;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -247,12 +255,26 @@ public class RobotContainer {
String[] sixBallAutoMiddlePaths = new String[]{
"SixBallMidComplete"
};
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths, false));
String[] eightBallAutoMiddlePaths = new String[]{
"EightBallMidComplete"
};
m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths));
m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths, false));
String[] driveOffLineForwardPaths = new String[]{
"DriveOffLineForward"
};
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths, false));
String[] driveOffLineBackwardPaths = new String[]{
"DriveOffLineBackward"
};
m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths, true));
}
/**
@@ -271,7 +293,9 @@ public class RobotContainer {
SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
//return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
return m_eightBallAutoMiddle.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_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
} catch (Exception e) {
System.err.println("ERROR");
@@ -314,7 +338,7 @@ public class RobotContainer {
return ramseteCommand;
}
public RamseteCommand[] buildPaths(String[] paths) {
public RamseteCommand[] buildPaths(String[] paths, boolean isReversed) {
RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length];
double[] times = new double[paths.length];
Trajectory initialTrajectory;
@@ -327,6 +351,7 @@ public class RobotContainer {
Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
initialTrajectory = trajectory;
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[0] = ramseteCommand;
times[0] = initialTrajectory.getTotalTimeSeconds();
src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java
+31
View File
@@ -0,0 +1,31 @@
/*----------------------------------------------------------------------------*/
/* 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.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.drive.TurnDegrees;
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 DriveOffLineBackward extends SequentialCommandGroup {
/**
* Creates a new DriveOffLineBackward.
*/
public DriveOffLineBackward(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
new TankDriveVelocity(drive, -1, -1, 2)
);
}
}
src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java
+29
View File
@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* 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 DriveOffLineForward extends SequentialCommandGroup {
/**
* Creates a new DriveOffLineForward.
*/
public DriveOffLineForward(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
+67
View File
@@ -0,0 +1,67 @@
/*----------------------------------------------------------------------------*/
/* 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.CommandBase;
import frc4388.robot.subsystems.Drive;
public class TankDriveVelocity extends CommandBase {
Drive m_drive;
double m_leftTargetVel;
double m_rightTargetVel;
double m_targetTime;
double m_firstTimeSec;
double m_currentTimeSec;
double m_diffSec;
/**
* Creates a new TankDriveVelocity.
*/
public TankDriveVelocity(Drive subsystem, double leftTargetVel, double rightTargetVel, double targetTime) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_leftTargetVel = leftTargetVel;
m_rightTargetVel = rightTargetVel;
m_targetTime = targetTime;
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_firstTimeSec = (System.currentTimeMillis() / 1000);
m_diffSec = 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;
if (m_diffSec < m_targetTime) {
m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_diffSec >= m_targetTime) {
return true;
}
return false;
}
}