From 480745d852588acc2574977ac8d0516862a65447 Mon Sep 17 00:00:00 2001 From: NogaRoze Date: Tue, 2 Mar 2021 20:02:20 -0600 Subject: [PATCH 1/6] We started the work on path following --- src/deploy/output/SixForward.wpilib.json | 1 + .../PathWeaver/Paths/SixForward.path | 3 + .../PathWeaver/pathweaver.json | 9 +++ .../frc/robot/commands/RunPathCommand.java | 57 +++++++++++++++++++ .../robot/subsystems/DriveBaseSubsystem.java | 11 +++- src/main/java/frc/robot/utilities/path.java | 32 +++++++++++ 6 files changed, 112 insertions(+), 1 deletion(-) create mode 100644 src/deploy/output/SixForward.wpilib.json create mode 100644 src/main/PathWeaverProject/PathWeaver/Paths/SixForward.path create mode 100644 src/main/PathWeaverProject/PathWeaver/pathweaver.json create mode 100644 src/main/java/frc/robot/commands/RunPathCommand.java create mode 100644 src/main/java/frc/robot/utilities/path.java diff --git a/src/deploy/output/SixForward.wpilib.json b/src/deploy/output/SixForward.wpilib.json new file mode 100644 index 0000000..1269da0 --- /dev/null +++ b/src/deploy/output/SixForward.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":9.999999999999998,"pose":{"translation":{"x":0.0,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.12499997033737249,"velocity":1.2499997033737247,"acceleration":10.000000000000002,"pose":{"translation":{"x":0.07812496292172,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.1767763612825469,"velocity":1.7677636128254688,"acceleration":10.0,"pose":{"translation":{"x":0.1562494095414877,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.21650497682420286,"velocity":2.1650497682420284,"acceleration":10.0,"pose":{"translation":{"x":0.23437202494824305,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.24999625680028503,"velocity":2.49996256800285,"acceleration":10.000000000000005,"pose":{"translation":{"x":0.31249064207077026,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.27950036230118047,"velocity":2.7950036230118047,"acceleration":9.999999999999995,"pose":{"translation":{"x":0.3906022626324557,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.30617089283798543,"velocity":3.061708928379854,"acceleration":10.0,"pose":{"translation":{"x":0.4687030781060457,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3306927548853784,"velocity":3.306927548853784,"acceleration":10.0,"pose":{"translation":{"x":0.5467884906684048,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3535118482187757,"velocity":3.5351184821877566,"acceleration":10.00000000000001,"pose":{"translation":{"x":0.6248531341552734,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3749375668070688,"velocity":3.749375668070688,"acceleration":10.0,"pose":{"translation":{"x":0.7028908950160258,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.39519487174517537,"velocity":3.9519487174517534,"acceleration":9.99999999999999,"pose":{"translation":{"x":0.7808949332684278,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.4144533033897536,"velocity":4.1445330338975355,"acceleration":10.0,"pose":{"translation":{"x":0.8588577034533955,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43284430817321656,"velocity":4.328443081732165,"acceleration":10.000000000000023,"pose":{"translation":{"x":0.9367709755897522,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45047216476248275,"velocity":4.5047216476248275,"acceleration":10.000000000000023,"pose":{"translation":{"x":1.014625856128987,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.46742118242758585,"velocity":4.674211824275859,"acceleration":10.0,"pose":{"translation":{"x":1.0924128089100122,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.48376061768480544,"velocity":4.8376061768480545,"acceleration":10.0,"pose":{"translation":{"x":1.1701216761139221,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.49954813566237044,"velocity":4.995481356623705,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.24774169921875,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5148323105544613,"velocity":5.1483231055446135,"acceleration":10.0,"pose":{"translation":{"x":1.3252615399542265,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5296544725113793,"velocity":5.296544725113794,"acceleration":10.0,"pose":{"translation":{"x":1.4026693012565374,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5440500984694483,"velocity":5.440500984694483,"acceleration":10.0,"pose":{"translation":{"x":1.479952548223082,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5580498775319693,"velocity":5.580498775319694,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.5570983290672302,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5716805394751655,"velocity":5.716805394751655,"acceleration":10.0,"pose":{"translation":{"x":1.6340931960730813,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5849655077951557,"velocity":5.849655077951557,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.7109232265502214,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5979254207321313,"velocity":5.979254207321314,"acceleration":10.0,"pose":{"translation":{"x":1.7875740437884815,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6105785515415186,"velocity":6.105785515415188,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.8640308380126953,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6229411508862546,"velocity":6.2294115088625475,"acceleration":10.0,"pose":{"translation":{"x":1.9402783873374574,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6350277283271779,"velocity":6.350277283271781,"acceleration":10.0,"pose":{"translation":{"x":2.016301078721881,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6468512856792286,"velocity":6.468512856792288,"acceleration":10.000000000000046,"pose":{"translation":{"x":2.0920829289243557,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.658423511952194,"velocity":6.584235119521943,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.167607605457306,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6697549473564114,"velocity":6.697549473564117,"acceleration":10.0,"pose":{"translation":{"x":2.2428584475419484,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6808551221901835,"velocity":6.808551221901839,"acceleration":10.0,"pose":{"translation":{"x":2.3178184870630503,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6917326751749819,"velocity":6.9173267517498225,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.392470469523687,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7023954548543148,"velocity":7.02395454854315,"acceleration":10.0,"pose":{"translation":{"x":2.466796875,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7128506069431314,"velocity":7.1285060694313165,"acceleration":10.0,"pose":{"translation":{"x":2.5407799390959553,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7231046499502128,"velocity":7.231046499502131,"acceleration":10.000000000000048,"pose":{"translation":{"x":2.614401673898101,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7331635409552664,"velocity":7.331635409552667,"acceleration":10.0,"pose":{"translation":{"x":2.6876438889303245,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7430327330755504,"velocity":7.430327330755507,"acceleration":9.99999999999995,"pose":{"translation":{"x":2.760488212108612,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7527172258817788,"velocity":7.5271722588177905,"acceleration":10.00000000000005,"pose":{"translation":{"x":2.8329161106958054,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7622216098033903,"velocity":7.622216098033905,"acceleration":10.0,"pose":{"translation":{"x":2.90490891225636,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7715501053866951,"velocity":7.715501053866954,"acceleration":-3.3717588560186713,"pose":{"translation":{"x":2.9764478256111033,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7807795440416799,"velocity":7.684381612345928,"acceleration":-10.0,"pose":{"translation":{"x":3.047513961791992,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7900192272742247,"velocity":7.59198478002048,"acceleration":-10.0,"pose":{"translation":{"x":3.118088354996871,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7993046414541025,"velocity":7.499130638221702,"acceleration":-10.0,"pose":{"translation":{"x":3.1881519835442305,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8086349335117604,"velocity":7.405827717645123,"acceleration":-10.0,"pose":{"translation":{"x":3.2576857908279635,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8180092132949669,"velocity":7.312084919813058,"acceleration":-10.0,"pose":{"translation":{"x":3.3266707062721252,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8274265535274705,"velocity":7.217911517488022,"acceleration":-10.000000000000052,"pose":{"translation":{"x":3.39508766628569,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.836885989741622,"velocity":7.123317155346507,"acceleration":-10.0,"pose":{"translation":{"x":3.462917635217309,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8463865201841579,"velocity":7.028311850921148,"acceleration":-9.999999999999947,"pose":{"translation":{"x":3.530141626310069,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8559271056942862,"velocity":6.9329059958198656,"acceleration":-10.0,"pose":{"translation":{"x":3.59674072265625,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8655066695531509,"velocity":6.837110357231218,"acceleration":-10.0,"pose":{"translation":{"x":3.6626960981520824,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8751240973036861,"velocity":6.7409360797258655,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.727989038452506,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8847782365397934,"velocity":6.644394687364793,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.7926009619259275,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8944678966636967,"velocity":6.547498086125758,"acceleration":-10.0,"pose":{"translation":{"x":3.8565134406089783,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9139488245368117,"velocity":6.352688807394609,"acceleration":-10.0,"pose":{"translation":{"x":3.9821672458201647,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9335565809593984,"velocity":6.156611243168741,"acceleration":-10.0,"pose":{"translation":{"x":4.104806900024414,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9532802335489408,"velocity":5.959374717273318,"acceleration":-10.00000000000003,"pose":{"translation":{"x":4.22429264895618,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9731082033060778,"velocity":5.761095019701946,"acceleration":-10.000000000000048,"pose":{"translation":{"x":4.340489208698273,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9930282476692652,"velocity":5.561894576070071,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.4532664362341166,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.013027440538092,"velocity":5.361902647381805,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.5625,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0330921489311773,"velocity":5.16125556345095,"acceleration":-10.000000000000018,"pose":{"translation":{"x":4.668072050437331,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.053208005884192,"velocity":4.960096993920802,"acceleration":-9.999999999999982,"pose":{"translation":{"x":4.769871890544891,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0733598791204104,"velocity":4.758578261558617,"acceleration":-10.0,"pose":{"translation":{"x":4.8677966464310884,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0935318349371121,"velocity":4.556858703391601,"acceleration":-10.0,"pose":{"translation":{"x":4.961751937866211,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1137070966419202,"velocity":4.35510608634352,"acceleration":-10.000000000000021,"pose":{"translation":{"x":5.0516525488346815,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.133867996738349,"velocity":4.153497085379233,"acceleration":-9.999999999999979,"pose":{"translation":{"x":5.137423098087311,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1539959218921958,"velocity":3.952217833840764,"acceleration":-10.0,"pose":{"translation":{"x":5.218998709693551,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1740712495003311,"velocity":3.7514645577594097,"acceleration":-10.000000000000012,"pose":{"translation":{"x":5.29632568359375,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.194073274417897,"velocity":3.5514443085837506,"acceleration":-10.0,"pose":{"translation":{"x":5.369362166151404,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.213980124061222,"velocity":3.3523758121505,"acceleration":-10.000000000000007,"pose":{"translation":{"x":5.438078820705414,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2534143609235189,"velocity":2.958033443527531,"acceleration":-10.000000000000004,"pose":{"translation":{"x":5.562501907348633,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2921714334605936,"velocity":2.5704627181567834,"acceleration":-9.999999999999995,"pose":{"translation":{"x":5.669636070728302,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3300217313168947,"velocity":2.1919597395937727,"acceleration":-10.0,"pose":{"translation":{"x":5.759765625,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3667016944400214,"velocity":1.8251601083625066,"acceleration":-10.0,"pose":{"translation":{"x":5.833439528942108,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.401903648667902,"velocity":1.4731405660837007,"acceleration":-10.0,"pose":{"translation":{"x":5.89149284362793,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4663168090582577,"velocity":0.8290089621801443,"acceleration":-9.999999999999998,"pose":{"translation":{"x":5.96563720703125,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.5492177052762721,"velocity":0.0,"acceleration":-9.999999999999998,"pose":{"translation":{"x":6.0,"y":26.9375},"rotation":{"radians":0.0}},"curvature":"NaN"}] \ No newline at end of file diff --git a/src/main/PathWeaverProject/PathWeaver/Paths/SixForward.path b/src/main/PathWeaverProject/PathWeaver/Paths/SixForward.path new file mode 100644 index 0000000..7c9bce3 --- /dev/null +++ b/src/main/PathWeaverProject/PathWeaver/Paths/SixForward.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +0.0,0.0,10.0,0.0,true,false, +6.0,0.0,0.0,0.0,true,false, diff --git a/src/main/PathWeaverProject/PathWeaver/pathweaver.json b/src/main/PathWeaverProject/PathWeaver/pathweaver.json new file mode 100644 index 0000000..3834e13 --- /dev/null +++ b/src/main/PathWeaverProject/PathWeaver/pathweaver.json @@ -0,0 +1,9 @@ +{ + "lengthUnit": "Meter", + "exportUnit": "Same as Project", + "maxVelocity": 12.0, + "maxAcceleration": 10.0, + "wheelBase": 28.0, + "gameName": "Infinite Recharge", + "outputDir": "../../../deploy" +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/RunPathCommand.java b/src/main/java/frc/robot/commands/RunPathCommand.java new file mode 100644 index 0000000..83f463c --- /dev/null +++ b/src/main/java/frc/robot/commands/RunPathCommand.java @@ -0,0 +1,57 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import frc.robot.subsystems.DriveBaseSubsystem; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; + +import java.io.*; +import java.nio.file.Path; + +public class RunPathCommand extends RamseteCommand { + /** Creates a new RunPathCommand. */ + + private final DriveBaseSubsystem mDriveBaseSubsystem; + private Trajectory mTrajectory = new Trajectory(); + private String mPathName; + public RunPathCommand(DriveBaseSubsystem driveBaseSubsystem, String pathName) { + super(); + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveBaseSubsystem); + mDriveBaseSubsystem = driveBaseSubsystem; + mPathName = pathName; + try { + Path path = Filesystem.getDeployDirectory().toPath().resolve(mPathName); + mTrajectory = TrajectoryUtil.fromPathweaverJson(path); + + } + catch(IOException ex) { + DriverStation.reportError("Could not find file: " + mPathName, ex.getStackTrace()); + } + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // 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() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java index 031cc5d..2572f6c 100644 --- a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java @@ -15,7 +15,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class DriveBaseSubsystem extends SubsystemBase { +import java.util.function.*; + +public class DriveBaseSubsystem extends SubsystemBase implements BiConsumer { /** * Creates a new DriveBaseSubsystem. */ @@ -25,6 +27,8 @@ public class DriveBaseSubsystem extends SubsystemBase { private final DifferentialDrive mDiffDrive; private boolean mIsArcadeDrive = true; + + public DriveBaseSubsystem(Joystick joystick) { mDriverJoystick = joystick; mMotorControllers[Constants.kDriveLeftFrontIndex] = new MotorController("Differential Left Front", Constants.kDriveLeftFront, Constants.kDriveBaseCurrentLimit); @@ -70,6 +74,11 @@ public void stopMotors() { mDiffDrive.arcadeDrive(0.0, 0.0); } + @Override + public void accept​(Double t, Double u) { + + } + @Override public void periodic() { diff --git a/src/main/java/frc/robot/utilities/path.java b/src/main/java/frc/robot/utilities/path.java new file mode 100644 index 0000000..b522939 --- /dev/null +++ b/src/main/java/frc/robot/utilities/path.java @@ -0,0 +1,32 @@ +package frc.robot.utilities; + +import java.util.ArrayList; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; +import edu.wpi.first.wpilibj.DriverStation; +import java.io.*; +import java.nio.file.Path; +import edu.wpi.first.wpilibj2.command.RamseteCommand; + +public class path { + private String mPathName; + private RamseteCommand mPathCommand; + public Trajectory mTrajectory = new Trajectory(); + + + public path(String pathName) { + mPathName = pathName; + try { + Path path = Filesystem.getDeployDirectory().toPath().resolve(mPathName); + mTrajectory = TrajectoryUtil.fromPathweaverJson(path); + } + catch(IOException ex) { + DriverStation.reportError("Could not find file: " + mPathName, ex.getStackTrace()); + } + mPathCommand = new RamseteCommand(mTrajectory, pose, follower, + kinematics, outputMetersPerSecond, requirements); + + } +} From 256551ff0cb6efe06486516f946032c107d4b715 Mon Sep 17 00:00:00 2001 From: SivonP Date: Tue, 9 Mar 2021 19:57:53 -0600 Subject: [PATCH 2/6] fixed path.java and the RamseteCommand --- src/main/java/frc/robot/Constants.java | 8 ++++ .../frc/robot/commands/RunPathCommand.java | 4 +- .../robot/subsystems/DriveBaseSubsystem.java | 39 +++++++++++++++---- src/main/java/frc/robot/utilities/path.java | 18 ++++++--- 4 files changed, 53 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 64ab53e..4f8ca4b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,5 +62,13 @@ public final class Constants { public static final int kRightJoystickAxisX = 4; public static final int kRightJoystickAxisY = 5; + public static final int kNeoEncoderResolution = 42; // Neo encoder has 42 counts per revolution + public static final double kDriveBaseWheelDiameter = 6.0; // in inches + public static final double kGearboxRatio = 9.7; // 9.7 motor rotations : 1 wheel rotation Reduction; slows the rotation down + public static final double kInchesToMeters = 0.0254; + public static final double kPositionConversionFactor = (kDriveBaseWheelDiameter * Math.PI * kGearboxRatio) * kInchesToMeters; // in meters + + public static final double kWheelBaseTrackWidthInches = 26; + public static final double kWheelBaseTrackWidthMeters = kWheelBaseTrackWidthInches * kInchesToMeters; } diff --git a/src/main/java/frc/robot/commands/RunPathCommand.java b/src/main/java/frc/robot/commands/RunPathCommand.java index 83f463c..f0ef22e 100644 --- a/src/main/java/frc/robot/commands/RunPathCommand.java +++ b/src/main/java/frc/robot/commands/RunPathCommand.java @@ -15,14 +15,14 @@ import java.io.*; import java.nio.file.Path; -public class RunPathCommand extends RamseteCommand { +public class RunPathCommand extends CommandBase { /** Creates a new RunPathCommand. */ private final DriveBaseSubsystem mDriveBaseSubsystem; private Trajectory mTrajectory = new Trajectory(); private String mPathName; public RunPathCommand(DriveBaseSubsystem driveBaseSubsystem, String pathName) { - super(); + // Use addRequirements() here to declare subsystem dependencies. addRequirements(driveBaseSubsystem); mDriveBaseSubsystem = driveBaseSubsystem; diff --git a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java index 2572f6c..3f6326a 100644 --- a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java @@ -9,15 +9,20 @@ import frc.robot.Constants; import frc.robot.MotorController; - +import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.*; -public class DriveBaseSubsystem extends SubsystemBase implements BiConsumer { +public class DriveBaseSubsystem extends SubsystemBase implements BiConsumer, Supplier { /** * Creates a new DriveBaseSubsystem. */ @@ -26,8 +31,9 @@ public class DriveBaseSubsystem extends SubsystemBase implements BiConsumer Date: Tue, 23 Mar 2021 18:42:13 -0500 Subject: [PATCH 3/6] finished accept function in the drivebase subsystem --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/robot/subsystems/DriveBaseSubsystem.java | 9 +++++++-- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/build.gradle b/build.gradle index 6f266d1..08379d0 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2021.1.2" + id "edu.wpi.first.GradleRIO" version "2021.2.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4f8ca4b..22cc5d0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,7 +66,7 @@ public final class Constants { public static final double kDriveBaseWheelDiameter = 6.0; // in inches public static final double kGearboxRatio = 9.7; // 9.7 motor rotations : 1 wheel rotation Reduction; slows the rotation down public static final double kInchesToMeters = 0.0254; - public static final double kPositionConversionFactor = (kDriveBaseWheelDiameter * Math.PI * kGearboxRatio) * kInchesToMeters; // in meters + public static final double kPositionConversionFactor = kDriveBaseWheelDiameter * Math.PI / kGearboxRatio * kInchesToMeters; // in meters public static final double kWheelBaseTrackWidthInches = 26; public static final double kWheelBaseTrackWidthMeters = kWheelBaseTrackWidthInches * kInchesToMeters; diff --git a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java index 3f6326a..a7586f9 100644 --- a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java @@ -9,6 +9,7 @@ import frc.robot.Constants; import frc.robot.MotorController; +import net.thefletcher.revrobotics.enums.ControlType; import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -22,6 +23,8 @@ import java.util.function.*; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + public class DriveBaseSubsystem extends SubsystemBase implements BiConsumer, Supplier { /** * Creates a new DriveBaseSubsystem. @@ -102,8 +105,10 @@ public void periodic() @Override public void accept(Double arg0, Double arg1) { - // TODO Auto-generated method stub - + //arg0 is left drivebase velocity in wheel mps, and arg1 is drivebase right velocity in wheel mps + //We do arg divided by the conversionFactorConstant to convert wheel speed mps to motor rpm + mMotorControllers[Constants.kDriveLeftFrontIndex].getSparkMax().setReference(arg0 / Constants.kPositionConversionFactor, ControlType.kVelocity); + mMotorControllers[Constants.kDriveRightFrontIndex].getSparkMax().setReference(arg1 / Constants.kPositionConversionFactor, ControlType.kVelocity); } @Override From 7454ceedddeb8f208453d24fbedb8b68a8d741ec Mon Sep 17 00:00:00 2001 From: SivonP Date: Tue, 23 Mar 2021 19:30:41 -0500 Subject: [PATCH 4/6] path is ready to be tested --- .../PathWeaver/pathweaver.json | 2 +- .../deploy/output/SixForward.wpilib.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 5 ++ .../frc/robot/commands/RunPathCommand.java | 57 ------------------- .../robot/subsystems/DriveBaseSubsystem.java | 9 ++- src/main/java/frc/robot/utilities/path.java | 8 +-- 6 files changed, 15 insertions(+), 68 deletions(-) rename src/{ => main}/deploy/output/SixForward.wpilib.json (62%) delete mode 100644 src/main/java/frc/robot/commands/RunPathCommand.java diff --git a/src/main/PathWeaverProject/PathWeaver/pathweaver.json b/src/main/PathWeaverProject/PathWeaver/pathweaver.json index 3834e13..379059a 100644 --- a/src/main/PathWeaverProject/PathWeaver/pathweaver.json +++ b/src/main/PathWeaverProject/PathWeaver/pathweaver.json @@ -5,5 +5,5 @@ "maxAcceleration": 10.0, "wheelBase": 28.0, "gameName": "Infinite Recharge", - "outputDir": "../../../deploy" + "outputDir": "../../deploy" } \ No newline at end of file diff --git a/src/deploy/output/SixForward.wpilib.json b/src/main/deploy/output/SixForward.wpilib.json similarity index 62% rename from src/deploy/output/SixForward.wpilib.json rename to src/main/deploy/output/SixForward.wpilib.json index 1269da0..6d30a8a 100644 --- a/src/deploy/output/SixForward.wpilib.json +++ b/src/main/deploy/output/SixForward.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":9.999999999999998,"pose":{"translation":{"x":0.0,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.12499997033737249,"velocity":1.2499997033737247,"acceleration":10.000000000000002,"pose":{"translation":{"x":0.07812496292172,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.1767763612825469,"velocity":1.7677636128254688,"acceleration":10.0,"pose":{"translation":{"x":0.1562494095414877,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.21650497682420286,"velocity":2.1650497682420284,"acceleration":10.0,"pose":{"translation":{"x":0.23437202494824305,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.24999625680028503,"velocity":2.49996256800285,"acceleration":10.000000000000005,"pose":{"translation":{"x":0.31249064207077026,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.27950036230118047,"velocity":2.7950036230118047,"acceleration":9.999999999999995,"pose":{"translation":{"x":0.3906022626324557,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.30617089283798543,"velocity":3.061708928379854,"acceleration":10.0,"pose":{"translation":{"x":0.4687030781060457,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3306927548853784,"velocity":3.306927548853784,"acceleration":10.0,"pose":{"translation":{"x":0.5467884906684048,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3535118482187757,"velocity":3.5351184821877566,"acceleration":10.00000000000001,"pose":{"translation":{"x":0.6248531341552734,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3749375668070688,"velocity":3.749375668070688,"acceleration":10.0,"pose":{"translation":{"x":0.7028908950160258,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.39519487174517537,"velocity":3.9519487174517534,"acceleration":9.99999999999999,"pose":{"translation":{"x":0.7808949332684278,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.4144533033897536,"velocity":4.1445330338975355,"acceleration":10.0,"pose":{"translation":{"x":0.8588577034533955,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43284430817321656,"velocity":4.328443081732165,"acceleration":10.000000000000023,"pose":{"translation":{"x":0.9367709755897522,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45047216476248275,"velocity":4.5047216476248275,"acceleration":10.000000000000023,"pose":{"translation":{"x":1.014625856128987,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.46742118242758585,"velocity":4.674211824275859,"acceleration":10.0,"pose":{"translation":{"x":1.0924128089100122,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.48376061768480544,"velocity":4.8376061768480545,"acceleration":10.0,"pose":{"translation":{"x":1.1701216761139221,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.49954813566237044,"velocity":4.995481356623705,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.24774169921875,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5148323105544613,"velocity":5.1483231055446135,"acceleration":10.0,"pose":{"translation":{"x":1.3252615399542265,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5296544725113793,"velocity":5.296544725113794,"acceleration":10.0,"pose":{"translation":{"x":1.4026693012565374,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5440500984694483,"velocity":5.440500984694483,"acceleration":10.0,"pose":{"translation":{"x":1.479952548223082,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5580498775319693,"velocity":5.580498775319694,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.5570983290672302,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5716805394751655,"velocity":5.716805394751655,"acceleration":10.0,"pose":{"translation":{"x":1.6340931960730813,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5849655077951557,"velocity":5.849655077951557,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.7109232265502214,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5979254207321313,"velocity":5.979254207321314,"acceleration":10.0,"pose":{"translation":{"x":1.7875740437884815,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6105785515415186,"velocity":6.105785515415188,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.8640308380126953,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6229411508862546,"velocity":6.2294115088625475,"acceleration":10.0,"pose":{"translation":{"x":1.9402783873374574,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6350277283271779,"velocity":6.350277283271781,"acceleration":10.0,"pose":{"translation":{"x":2.016301078721881,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6468512856792286,"velocity":6.468512856792288,"acceleration":10.000000000000046,"pose":{"translation":{"x":2.0920829289243557,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.658423511952194,"velocity":6.584235119521943,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.167607605457306,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6697549473564114,"velocity":6.697549473564117,"acceleration":10.0,"pose":{"translation":{"x":2.2428584475419484,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6808551221901835,"velocity":6.808551221901839,"acceleration":10.0,"pose":{"translation":{"x":2.3178184870630503,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6917326751749819,"velocity":6.9173267517498225,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.392470469523687,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7023954548543148,"velocity":7.02395454854315,"acceleration":10.0,"pose":{"translation":{"x":2.466796875,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7128506069431314,"velocity":7.1285060694313165,"acceleration":10.0,"pose":{"translation":{"x":2.5407799390959553,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7231046499502128,"velocity":7.231046499502131,"acceleration":10.000000000000048,"pose":{"translation":{"x":2.614401673898101,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7331635409552664,"velocity":7.331635409552667,"acceleration":10.0,"pose":{"translation":{"x":2.6876438889303245,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7430327330755504,"velocity":7.430327330755507,"acceleration":9.99999999999995,"pose":{"translation":{"x":2.760488212108612,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7527172258817788,"velocity":7.5271722588177905,"acceleration":10.00000000000005,"pose":{"translation":{"x":2.8329161106958054,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7622216098033903,"velocity":7.622216098033905,"acceleration":10.0,"pose":{"translation":{"x":2.90490891225636,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7715501053866951,"velocity":7.715501053866954,"acceleration":-3.3717588560186713,"pose":{"translation":{"x":2.9764478256111033,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7807795440416799,"velocity":7.684381612345928,"acceleration":-10.0,"pose":{"translation":{"x":3.047513961791992,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7900192272742247,"velocity":7.59198478002048,"acceleration":-10.0,"pose":{"translation":{"x":3.118088354996871,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7993046414541025,"velocity":7.499130638221702,"acceleration":-10.0,"pose":{"translation":{"x":3.1881519835442305,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8086349335117604,"velocity":7.405827717645123,"acceleration":-10.0,"pose":{"translation":{"x":3.2576857908279635,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8180092132949669,"velocity":7.312084919813058,"acceleration":-10.0,"pose":{"translation":{"x":3.3266707062721252,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8274265535274705,"velocity":7.217911517488022,"acceleration":-10.000000000000052,"pose":{"translation":{"x":3.39508766628569,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.836885989741622,"velocity":7.123317155346507,"acceleration":-10.0,"pose":{"translation":{"x":3.462917635217309,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8463865201841579,"velocity":7.028311850921148,"acceleration":-9.999999999999947,"pose":{"translation":{"x":3.530141626310069,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8559271056942862,"velocity":6.9329059958198656,"acceleration":-10.0,"pose":{"translation":{"x":3.59674072265625,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8655066695531509,"velocity":6.837110357231218,"acceleration":-10.0,"pose":{"translation":{"x":3.6626960981520824,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8751240973036861,"velocity":6.7409360797258655,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.727989038452506,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8847782365397934,"velocity":6.644394687364793,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.7926009619259275,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8944678966636967,"velocity":6.547498086125758,"acceleration":-10.0,"pose":{"translation":{"x":3.8565134406089783,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9139488245368117,"velocity":6.352688807394609,"acceleration":-10.0,"pose":{"translation":{"x":3.9821672458201647,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9335565809593984,"velocity":6.156611243168741,"acceleration":-10.0,"pose":{"translation":{"x":4.104806900024414,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9532802335489408,"velocity":5.959374717273318,"acceleration":-10.00000000000003,"pose":{"translation":{"x":4.22429264895618,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9731082033060778,"velocity":5.761095019701946,"acceleration":-10.000000000000048,"pose":{"translation":{"x":4.340489208698273,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9930282476692652,"velocity":5.561894576070071,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.4532664362341166,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.013027440538092,"velocity":5.361902647381805,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.5625,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0330921489311773,"velocity":5.16125556345095,"acceleration":-10.000000000000018,"pose":{"translation":{"x":4.668072050437331,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.053208005884192,"velocity":4.960096993920802,"acceleration":-9.999999999999982,"pose":{"translation":{"x":4.769871890544891,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0733598791204104,"velocity":4.758578261558617,"acceleration":-10.0,"pose":{"translation":{"x":4.8677966464310884,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0935318349371121,"velocity":4.556858703391601,"acceleration":-10.0,"pose":{"translation":{"x":4.961751937866211,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1137070966419202,"velocity":4.35510608634352,"acceleration":-10.000000000000021,"pose":{"translation":{"x":5.0516525488346815,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.133867996738349,"velocity":4.153497085379233,"acceleration":-9.999999999999979,"pose":{"translation":{"x":5.137423098087311,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1539959218921958,"velocity":3.952217833840764,"acceleration":-10.0,"pose":{"translation":{"x":5.218998709693551,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1740712495003311,"velocity":3.7514645577594097,"acceleration":-10.000000000000012,"pose":{"translation":{"x":5.29632568359375,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.194073274417897,"velocity":3.5514443085837506,"acceleration":-10.0,"pose":{"translation":{"x":5.369362166151404,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.213980124061222,"velocity":3.3523758121505,"acceleration":-10.000000000000007,"pose":{"translation":{"x":5.438078820705414,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2534143609235189,"velocity":2.958033443527531,"acceleration":-10.000000000000004,"pose":{"translation":{"x":5.562501907348633,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2921714334605936,"velocity":2.5704627181567834,"acceleration":-9.999999999999995,"pose":{"translation":{"x":5.669636070728302,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3300217313168947,"velocity":2.1919597395937727,"acceleration":-10.0,"pose":{"translation":{"x":5.759765625,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3667016944400214,"velocity":1.8251601083625066,"acceleration":-10.0,"pose":{"translation":{"x":5.833439528942108,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.401903648667902,"velocity":1.4731405660837007,"acceleration":-10.0,"pose":{"translation":{"x":5.89149284362793,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4663168090582577,"velocity":0.8290089621801443,"acceleration":-9.999999999999998,"pose":{"translation":{"x":5.96563720703125,"y":26.9375},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.5492177052762721,"velocity":0.0,"acceleration":-9.999999999999998,"pose":{"translation":{"x":6.0,"y":26.9375},"rotation":{"radians":0.0}},"curvature":"NaN"}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":9.999999999999998,"pose":{"translation":{"x":0.0,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.12499997033737249,"velocity":1.2499997033737247,"acceleration":10.000000000000002,"pose":{"translation":{"x":0.07812496292172,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.1767763612825469,"velocity":1.7677636128254688,"acceleration":10.0,"pose":{"translation":{"x":0.1562494095414877,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.21650497682420286,"velocity":2.1650497682420284,"acceleration":10.0,"pose":{"translation":{"x":0.23437202494824305,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.24999625680028503,"velocity":2.49996256800285,"acceleration":10.000000000000005,"pose":{"translation":{"x":0.31249064207077026,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.27950036230118047,"velocity":2.7950036230118047,"acceleration":9.999999999999995,"pose":{"translation":{"x":0.3906022626324557,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.30617089283798543,"velocity":3.061708928379854,"acceleration":10.0,"pose":{"translation":{"x":0.4687030781060457,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3306927548853784,"velocity":3.306927548853784,"acceleration":10.0,"pose":{"translation":{"x":0.5467884906684048,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3535118482187757,"velocity":3.5351184821877566,"acceleration":10.00000000000001,"pose":{"translation":{"x":0.6248531341552734,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3749375668070688,"velocity":3.749375668070688,"acceleration":10.0,"pose":{"translation":{"x":0.7028908950160258,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.39519487174517537,"velocity":3.9519487174517534,"acceleration":9.99999999999999,"pose":{"translation":{"x":0.7808949332684278,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.4144533033897536,"velocity":4.1445330338975355,"acceleration":10.0,"pose":{"translation":{"x":0.8588577034533955,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43284430817321656,"velocity":4.328443081732165,"acceleration":10.000000000000023,"pose":{"translation":{"x":0.9367709755897522,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45047216476248275,"velocity":4.5047216476248275,"acceleration":10.000000000000023,"pose":{"translation":{"x":1.014625856128987,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.46742118242758585,"velocity":4.674211824275859,"acceleration":10.0,"pose":{"translation":{"x":1.0924128089100122,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.48376061768480544,"velocity":4.8376061768480545,"acceleration":10.0,"pose":{"translation":{"x":1.1701216761139221,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.49954813566237044,"velocity":4.995481356623705,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.24774169921875,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5148323105544613,"velocity":5.1483231055446135,"acceleration":10.0,"pose":{"translation":{"x":1.3252615399542265,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5296544725113793,"velocity":5.296544725113794,"acceleration":10.0,"pose":{"translation":{"x":1.4026693012565374,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5440500984694483,"velocity":5.440500984694483,"acceleration":10.0,"pose":{"translation":{"x":1.479952548223082,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5580498775319693,"velocity":5.580498775319694,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.5570983290672302,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5716805394751655,"velocity":5.716805394751655,"acceleration":10.0,"pose":{"translation":{"x":1.6340931960730813,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5849655077951557,"velocity":5.849655077951557,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.7109232265502214,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5979254207321313,"velocity":5.979254207321314,"acceleration":10.0,"pose":{"translation":{"x":1.7875740437884815,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6105785515415186,"velocity":6.105785515415188,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.8640308380126953,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6229411508862546,"velocity":6.2294115088625475,"acceleration":10.0,"pose":{"translation":{"x":1.9402783873374574,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6350277283271779,"velocity":6.350277283271781,"acceleration":10.0,"pose":{"translation":{"x":2.016301078721881,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6468512856792286,"velocity":6.468512856792288,"acceleration":10.000000000000046,"pose":{"translation":{"x":2.0920829289243557,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.658423511952194,"velocity":6.584235119521943,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.167607605457306,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6697549473564114,"velocity":6.697549473564117,"acceleration":10.0,"pose":{"translation":{"x":2.2428584475419484,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6808551221901835,"velocity":6.808551221901839,"acceleration":10.0,"pose":{"translation":{"x":2.3178184870630503,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6917326751749819,"velocity":6.9173267517498225,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.392470469523687,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7023954548543148,"velocity":7.02395454854315,"acceleration":10.0,"pose":{"translation":{"x":2.466796875,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7128506069431314,"velocity":7.1285060694313165,"acceleration":10.0,"pose":{"translation":{"x":2.5407799390959553,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7231046499502128,"velocity":7.231046499502131,"acceleration":10.000000000000048,"pose":{"translation":{"x":2.614401673898101,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7331635409552664,"velocity":7.331635409552667,"acceleration":10.0,"pose":{"translation":{"x":2.6876438889303245,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7430327330755504,"velocity":7.430327330755507,"acceleration":9.99999999999995,"pose":{"translation":{"x":2.760488212108612,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7527172258817788,"velocity":7.5271722588177905,"acceleration":10.00000000000005,"pose":{"translation":{"x":2.8329161106958054,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7622216098033903,"velocity":7.622216098033905,"acceleration":10.0,"pose":{"translation":{"x":2.90490891225636,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7715501053866951,"velocity":7.715501053866954,"acceleration":-3.3717588560186713,"pose":{"translation":{"x":2.9764478256111033,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7807795440416799,"velocity":7.684381612345928,"acceleration":-10.0,"pose":{"translation":{"x":3.047513961791992,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7900192272742247,"velocity":7.59198478002048,"acceleration":-10.0,"pose":{"translation":{"x":3.118088354996871,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7993046414541025,"velocity":7.499130638221702,"acceleration":-10.0,"pose":{"translation":{"x":3.1881519835442305,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8086349335117604,"velocity":7.405827717645123,"acceleration":-10.0,"pose":{"translation":{"x":3.2576857908279635,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8180092132949669,"velocity":7.312084919813058,"acceleration":-10.0,"pose":{"translation":{"x":3.3266707062721252,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8274265535274705,"velocity":7.217911517488022,"acceleration":-10.000000000000052,"pose":{"translation":{"x":3.39508766628569,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.836885989741622,"velocity":7.123317155346507,"acceleration":-10.0,"pose":{"translation":{"x":3.462917635217309,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8463865201841579,"velocity":7.028311850921148,"acceleration":-9.999999999999947,"pose":{"translation":{"x":3.530141626310069,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8559271056942862,"velocity":6.9329059958198656,"acceleration":-10.0,"pose":{"translation":{"x":3.59674072265625,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8655066695531509,"velocity":6.837110357231218,"acceleration":-10.0,"pose":{"translation":{"x":3.6626960981520824,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8751240973036861,"velocity":6.7409360797258655,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.727989038452506,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8847782365397934,"velocity":6.644394687364793,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.7926009619259275,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8944678966636967,"velocity":6.547498086125758,"acceleration":-10.0,"pose":{"translation":{"x":3.8565134406089783,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9139488245368117,"velocity":6.352688807394609,"acceleration":-10.0,"pose":{"translation":{"x":3.9821672458201647,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9335565809593984,"velocity":6.156611243168741,"acceleration":-10.0,"pose":{"translation":{"x":4.104806900024414,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9532802335489408,"velocity":5.959374717273318,"acceleration":-10.00000000000003,"pose":{"translation":{"x":4.22429264895618,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9731082033060778,"velocity":5.761095019701946,"acceleration":-10.000000000000048,"pose":{"translation":{"x":4.340489208698273,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9930282476692652,"velocity":5.561894576070071,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.4532664362341166,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.013027440538092,"velocity":5.361902647381805,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.5625,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0330921489311773,"velocity":5.16125556345095,"acceleration":-10.000000000000018,"pose":{"translation":{"x":4.668072050437331,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.053208005884192,"velocity":4.960096993920802,"acceleration":-9.999999999999982,"pose":{"translation":{"x":4.769871890544891,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0733598791204104,"velocity":4.758578261558617,"acceleration":-10.0,"pose":{"translation":{"x":4.8677966464310884,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0935318349371121,"velocity":4.556858703391601,"acceleration":-10.0,"pose":{"translation":{"x":4.961751937866211,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1137070966419202,"velocity":4.35510608634352,"acceleration":-10.000000000000021,"pose":{"translation":{"x":5.0516525488346815,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.133867996738349,"velocity":4.153497085379233,"acceleration":-9.999999999999979,"pose":{"translation":{"x":5.137423098087311,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1539959218921958,"velocity":3.952217833840764,"acceleration":-10.0,"pose":{"translation":{"x":5.218998709693551,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1740712495003311,"velocity":3.7514645577594097,"acceleration":-10.000000000000012,"pose":{"translation":{"x":5.29632568359375,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.194073274417897,"velocity":3.5514443085837506,"acceleration":-10.0,"pose":{"translation":{"x":5.369362166151404,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.213980124061222,"velocity":3.3523758121505,"acceleration":-10.000000000000007,"pose":{"translation":{"x":5.438078820705414,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2534143609235189,"velocity":2.958033443527531,"acceleration":-10.000000000000004,"pose":{"translation":{"x":5.562501907348633,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2921714334605936,"velocity":2.5704627181567834,"acceleration":-9.999999999999995,"pose":{"translation":{"x":5.669636070728302,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3300217313168947,"velocity":2.1919597395937727,"acceleration":-10.0,"pose":{"translation":{"x":5.759765625,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3667016944400214,"velocity":1.8251601083625066,"acceleration":-10.0,"pose":{"translation":{"x":5.833439528942108,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.401903648667902,"velocity":1.4731405660837007,"acceleration":-10.0,"pose":{"translation":{"x":5.89149284362793,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4663168090582577,"velocity":0.8290089621801443,"acceleration":-9.999999999999998,"pose":{"translation":{"x":5.96563720703125,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.5492177052762721,"velocity":0.0,"acceleration":-9.999999999999998,"pose":{"translation":{"x":6.0,"y":8.21055},"rotation":{"radians":0.0}},"curvature":"NaN"}] \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b1a756a..e17b7c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.XboxController; import frc.robot.commands.ShootCommandGroup; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.utilities.path; import frc.robot.commands.DriveBaseTeleopCommand; import frc.robot.commands.IntakeSpinMotorBackwardCommand; import frc.robot.commands.IntakeSpinMotorForwardCommand; @@ -51,6 +52,9 @@ public class RobotContainer { private final ShootCommandGroup mShootCommandGroup = new ShootCommandGroup(mShooterSubsystem, mHopperSubsystem); private final StartEndCommand mIntakeExtendCommand = new StartEndCommand(mIntakeSubsystem::setIntakeExtended, mIntakeSubsystem::setIntakeRetracted); + //Paths + private final path mSixFeetForward = new path("output/SixForward.wpilib.json", mDriveBaseSubsystem); + public RobotContainer() { // Configure the button bindings for (int i = 1; i < mButtons.length; i++) { @@ -76,6 +80,7 @@ private void configureButtonBindings() { mButtons[Constants.kAButton].whenPressed(mSwitchDriveModeCommand); //The buttons below are just for testing functionality mButtons[Constants.kRightTriggerButton].whileHeld(mCycleHopperCommand); + mButtons[Constants.kLeftTriggerButton].whileHeld(mSixFeetForward.getPathCommand()); } diff --git a/src/main/java/frc/robot/commands/RunPathCommand.java b/src/main/java/frc/robot/commands/RunPathCommand.java deleted file mode 100644 index f0ef22e..0000000 --- a/src/main/java/frc/robot/commands/RunPathCommand.java +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import frc.robot.subsystems.DriveBaseSubsystem; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.trajectory.Trajectory; -import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; - -import java.io.*; -import java.nio.file.Path; - -public class RunPathCommand extends CommandBase { - /** Creates a new RunPathCommand. */ - - private final DriveBaseSubsystem mDriveBaseSubsystem; - private Trajectory mTrajectory = new Trajectory(); - private String mPathName; - public RunPathCommand(DriveBaseSubsystem driveBaseSubsystem, String pathName) { - - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(driveBaseSubsystem); - mDriveBaseSubsystem = driveBaseSubsystem; - mPathName = pathName; - try { - Path path = Filesystem.getDeployDirectory().toPath().resolve(mPathName); - mTrajectory = TrajectoryUtil.fromPathweaverJson(path); - - } - catch(IOException ex) { - DriverStation.reportError("Could not find file: " + mPathName, ex.getStackTrace()); - } - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // 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() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java index d4d3c32..7f6393d 100644 --- a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java @@ -17,13 +17,11 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.*; import com.revrobotics.ControlType; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; public class DriveBaseSubsystem extends SubsystemBase implements BiConsumer, Supplier { /** @@ -40,10 +38,10 @@ public class DriveBaseSubsystem extends SubsystemBase implements BiConsumer Date: Tue, 23 Mar 2021 19:32:40 -0500 Subject: [PATCH 5/6] corrected the values in pathweaver --- src/main/PathWeaverProject/PathWeaver/pathweaver.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/PathWeaverProject/PathWeaver/pathweaver.json b/src/main/PathWeaverProject/PathWeaver/pathweaver.json index 379059a..2c1efaa 100644 --- a/src/main/PathWeaverProject/PathWeaver/pathweaver.json +++ b/src/main/PathWeaverProject/PathWeaver/pathweaver.json @@ -1,9 +1,9 @@ { "lengthUnit": "Meter", "exportUnit": "Same as Project", - "maxVelocity": 12.0, - "maxAcceleration": 10.0, - "wheelBase": 28.0, + "maxVelocity": 3.6576, + "maxAcceleration": 3.048, + "wheelBase": 0.6604, "gameName": "Infinite Recharge", "outputDir": "../../deploy" } \ No newline at end of file From 07b451ac08bc1af3a838f584d020873844563e5e Mon Sep 17 00:00:00 2001 From: SivonP Date: Tue, 23 Mar 2021 19:46:23 -0500 Subject: [PATCH 6/6] cleaned MotorController PID constructors --- src/main/java/frc/robot/MotorController.java | 28 +++++++++++--------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/MotorController.java b/src/main/java/frc/robot/MotorController.java index 3f0df32..ef37eb1 100644 --- a/src/main/java/frc/robot/MotorController.java +++ b/src/main/java/frc/robot/MotorController.java @@ -37,20 +37,24 @@ public MotorController(String name, int deviceID) { mSparkMax.restoreFactoryDefaults(); } - //Todo: we must add constants for PID values once we have found the right values - public MotorController(String name, int deviceID, int smartCurrentLimit, boolean... enablePid) { - this(name, deviceID); //calls the constructor for correct arguements + public MotorController(String name, int deviceID, int smartCurrentLimit) { + this(name, deviceID); //Current limiting is required to prevent brown outs mSparkMax.setSmartCurrentLimit(smartCurrentLimit); - //If enablePid has any number of booleans greater than 0 we are enabling pid - if (enablePid.length > 0) - { - mP = SmartDashboard.getNumber(mName + " P Value", 1.0); - mI = SmartDashboard.getNumber(mName + " I Value", 0.0); - mD = SmartDashboard.getNumber(mName + " D Value", 0.0); - mPIDController = mSparkMax.getPIDController(); - setPID(); - } + } + + public MotorController(String name, int deviceID, int smartCurrentLimit, boolean enablePid) { + this(name, deviceID, smartCurrentLimit, SmartDashboard.getNumber(name + " P Value", 1.0), + SmartDashboard.getNumber(name + " I Value", 0.0), SmartDashboard.getNumber(name + " D Value", 0.0)); //calls the constructor for correct arguments + } + + public MotorController(String name, int deviceID, int smartCurrentLimit, double P, double I, double D) { + this(name, deviceID, smartCurrentLimit); //calls the constructor for correct arguements + mP = P; + mI = I; + mD = D; + mPIDController = mSparkMax.getPIDController(); + setPID(); mSparkMax.setOpenLoopRampRate(.1); }