diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..7b33f2f --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.711, + "robotLength": 0.711, + "holonomicMode": false, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/one cube.auto b/src/main/deploy/pathplanner/autos/one cube.auto new file mode 100644 index 0000000..29ae382 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/one cube.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.59, + "y": 3.73 + }, + "rotation": -90.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start_To_Perimiter (1)" + } + }, + { + "type": "path", + "data": { + "pathName": "perimiter_To_Back_of_Cube(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Back of Cube into home(3)" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Back of Cube into home(3).path b/src/main/deploy/pathplanner/paths/Back of Cube into home(3).path new file mode 100644 index 0000000..b4d12b0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Back of Cube into home(3).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.081007635872117, + "y": 2.146511524671921 + }, + "prevControl": null, + "nextControl": { + "x": 3.859055001204621, + "y": 2.146511524671921 + }, + "isLocked": false, + "linkedName": "behind cube#1" + }, + { + "anchor": { + "x": 0.8429091763679902, + "y": 2.0946366066713966 + }, + "prevControl": { + "x": 2.0875571473823786, + "y": 2.0946366066713966 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "home1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 0, + "rotateFast": false + }, + "reversed": true, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cube 1 to perimiter (4).path b/src/main/deploy/pathplanner/paths/Cube 1 to perimiter (4).path new file mode 100644 index 0000000..f444ad4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cube 1 to perimiter (4).path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8429091763679902, + "y": 2.0946366066713966 + }, + "prevControl": null, + "nextControl": { + "x": 4.766251005253288, + "y": 3.648322422343152 + }, + "isLocked": false, + "linkedName": "home1" + }, + { + "anchor": { + "x": 5.074084880795034, + "y": 3.616239874583321 + }, + "prevControl": { + "x": 4.572779876293277, + "y": 3.445484258286135 + }, + "nextControl": { + "x": 5.215686197132219, + "y": 3.6644724272658546 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.196224237992037, + "y": 3.826728224988743 + }, + "prevControl": { + "x": 4.928550983072929, + "y": 3.826728224988743 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start_To_Perimiter (1).path b/src/main/deploy/pathplanner/paths/Start_To_Perimiter (1).path new file mode 100644 index 0000000..f012116 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start_To_Perimiter (1).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.59, + "y": 3.729931640783178 + }, + "prevControl": null, + "nextControl": { + "x": 0.5900000000000002, + "y": 2.4670756793249056 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.098419859252312, + "y": 2.623962929928345 + }, + "prevControl": { + "x": 0.7683766317023171, + "y": 2.657898350931721 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "The actual end of perimiter" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/perimiter_To_Back_of_Cube(2).path b/src/main/deploy/pathplanner/paths/perimiter_To_Back_of_Cube(2).path new file mode 100644 index 0000000..4cc9a4e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/perimiter_To_Back_of_Cube(2).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.098419859252312, + "y": 2.623962929928345 + }, + "prevControl": null, + "nextControl": { + "x": 5.0985432489528355, + "y": 2.386063247713733 + }, + "isLocked": false, + "linkedName": "The actual end of perimiter" + }, + { + "anchor": { + "x": 5.081007635872117, + "y": 2.146511524671921 + }, + "prevControl": { + "x": 4.6996176702990855, + "y": 2.146511524671921 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "behind cube#1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ 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 847cf92..f2a1279 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,8 @@ import frc.robot.subsystems.drive.DriveIOReal; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.rollers.RollerIO; +import frc.robot.subsystems.drive.rollers.Rollers; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; import frc.robot.util.PoseManager; @@ -36,10 +38,10 @@ public class RobotContainer { // Subsystems private final Drive drive; - + public CommandXboxController controller; // Pose Manager private final PoseManager poseManager = new PoseManager(); - +private Rollers rollerIO; // Controller private final CommandXboxController driver = new CommandXboxController(0); @@ -94,6 +96,10 @@ private void configureButtonBindings() { // Driver controls + controller.x().whileTrue(rollerIO.run_Flywheel_Full_Speed()); + controller.y().whileTrue(rollerIO.run_Flywheel_Half_Speed()); + controller.a().whileTrue(rollerIO.run_Flywheel_x_Speed(5, 4)); + controller.b().onTrue(rollerIO.Stop_Fly_Wheels()); } /** Updates the alerts for disconnected controllers. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a942fc2..9cb675c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -8,6 +8,8 @@ import frc.robot.util.PoseManager; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; +import frc.robot.util.loggedShuffleboardClasses.*; +import frc.robot.util.loggedShuffleboardClasses.DriveIOInputsAutoLogged; public class Drive extends SubsystemBase { private final DriveIO io; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOSim.java b/src/main/java/frc/robot/subsystems/drive/DriveIOSim.java index 7be6186..68a30fa 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOSim.java @@ -33,7 +33,6 @@ public class DriveIOSim implements DriveIO { // l and r position: 0.005 m VecBuilder.fill(0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005)); - private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0); @@ -56,7 +55,8 @@ public DriveIOSim() { m_leftLeader.addFollower(m_leftFollower); m_rightLeader.addFollower(m_rightFollower); } - @Override + + @Override public void updateInputs(DriveIOInputs inputs) { // TODO: we will have encoders @@ -67,7 +67,6 @@ public void stopDriveTrain() { driveTrain.setInputs(0, 0); } - @Override public void arcadeDrive(double xSpeed, double omegaRotation) { @@ -75,7 +74,7 @@ public void arcadeDrive(double xSpeed, double omegaRotation) { } /** Sets speeds to the drivetrain motors. */ - //TODO there is a unit's conflict + // TODO there is a unit's conflict private void setSpeeds(DifferentialDriveWheelSpeeds speeds) { final double leftFeedforward = m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); diff --git a/src/main/java/frc/robot/subsystems/drive/Rollers/ROller_IO_Real b/src/main/java/frc/robot/subsystems/drive/Rollers/ROller_IO_Real new file mode 100644 index 0000000..ce4fa5e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Rollers/ROller_IO_Real @@ -0,0 +1,52 @@ +package frc.robot.subsystems.drive; +package frc.robot.subsystems.Roller_Constants_Dvid; +import org.littletonrobotics.junction.AutoLog; + +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.SparkPIDController; +import com.revrobotics.ReletiveEncoder; + +import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Default; + +public class RollerIOReal implements DriveIO { +// decalre motors +private int +private final CANSparkMax rightMotor = + new CANSparkMax(Roller_Constants_Dvid.rightMotorID, MotorType.kBrushless ); + private final CANcoder rightEncoder = new CANcoder(0); +private final CANSparkMax leftMotor = + new CANSparkMax(Roller_Constants_Dvid.rightMotorID, MotorType.kBrushless ); + private final CANcoder leftEncoder = new CANcoder(1); + + + + @Override + public void handle_Input(Rolle_IO_Inputs roller_Inputs){ + + roller_Inputs.r_Rot_Speed = rightEncoder.getVelocity(); + + roller_Inputs.l_Rot_Speed = leftEncoder.getVelocity(); + + roller_Inputs.r_Applied_Volts = rightMotor.getAppliedOutput() * rightMotor.GetBusVoltage(); + + roller_Inputs.l_Applied_Volts = leftMotor.getAppliedOutput() * leftMotor.GetBusVoltage(); + } + @Override + public default void run_L_Motor(double speed_In_Volts){ + + leftMotor.runMotor(speed_In_Volts); + } + @Override + public default void run_R_Motor(double speed_In_Volts){ + rightMotor.runMotor(speed_In_Volts); + } + @Override + public default void stop_Motor(){ + rightMotor.stopMotor(); + leftMotor.stopMotor(); + } + + + + +} diff --git a/src/main/java/frc/robot/subsystems/drive/Rollers/RollerIO.java b/src/main/java/frc/robot/subsystems/drive/Rollers/RollerIO.java new file mode 100644 index 0000000..e04cfdb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Rollers/RollerIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.drive.rollers; + +import org.littletonrobotics.junction.AutoLog; + +public interface RollerIO { + @AutoLog + public static class Rolle_IO_Inputs { + + public double r_Rot_Speed; // rotations per second + public double l_Rot_Speed; + public double r_Applied_Volts; // in volts + public double l_Applied_Volts; + } + + public default void handle_Input(Rolle_IO_Inputs roller_Inputs) {} + + public default void run_L_Motor(double speed_In_Volts) {} + + public default void run_R_Motor(double speed_In_Volts) {} + + public default void stop_Motor() {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/Rollers/Roller_Constants_Dvid b/src/main/java/frc/robot/subsystems/drive/Rollers/Roller_Constants_Dvid new file mode 100644 index 0000000..9be20dc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Rollers/Roller_Constants_Dvid @@ -0,0 +1,12 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; + +public final class DriveConstants { + public static final int rightMotorID = 0; + // TODO: Update Ids to 1 + public static final double flywheel_Full_Speed = 6; + public static final int leftMotorID = 1; + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Rollers/Rollers.java b/src/main/java/frc/robot/subsystems/drive/Rollers/Rollers.java new file mode 100644 index 0000000..f82b9c2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Rollers/Rollers.java @@ -0,0 +1,76 @@ +package frc.robot.subsystems.drive.rollers; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.drive.rollers.RollerIO; +import frc.robot.subsystems.drive.GyroIO; +import frc.robot.util.PoseManager; +//import frc.robot.subsystems.drive.Rollers.Roller_Constants_Dvid; + +public class Rollers extends SubsystemBase{ + + private final RollerIO io; + public double speed; +// private final rollerIOAutoLogged rollerInputs; + + + + public Rollers(RollerIO io, GyroIO gyroIO, PoseManager poseManager) { + this.io = io; + // this.gyroIO = gyroIO; + // this.poseManager = poseManager; + } + + + public void periodic(){ + + } + + private void run_volt_Left_Motor(double volts){ + io.run_L_Motor(volts); + } + + private void run_volt_Right_Motor(double volts){ + io.run_R_Motor(volts); + } + private void run_Both_Motors(double volts){ + io.run_R_Motor(volts); + io.run_L_Motor(volts); + } + + + private void stop_Both_Motors(){ + io.stop_Motor(); + } + + public Command run_Flywheel_Full_Speed (){ + return runOnce (() -> { + run_Both_Motors(speed); + }); + } + public Command run_Flywheel_Half_Speed (){ + return runOnce (() -> { + run_Both_Motors(speed/2); + }); + } + public Command run_Flywheel_x_Speed (double r_Speed, double l_Speed){ + return runOnce (() -> { + run_volt_Left_Motor(l_Speed); + run_volt_Right_Motor(r_Speed); + }); + } + public Command Stop_Fly_Wheels(){ + return runOnce(() -> { + stop_Both_Motors(); + }); + } + /* Command notatiton + public Command (Name) (){ return [run command](() -> { + [method] + }) + + } + */ + +} +