diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Libby.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Libby.java deleted file mode 100644 index 635921aa9efd..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Libby.java +++ /dev/null @@ -1,70 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.arcrobotics.ftclib.command.CommandOpMode; -import com.arcrobotics.ftclib.command.RunCommand; -import com.arcrobotics.ftclib.drivebase.MecanumDrive; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.subsystems.DefaultDrive; -import org.firstinspires.ftc.teamcode.subsystems.DriveSubsystem; - -@TeleOp -public class Libby extends CommandOpMode { - // ATTENTION: found out that this mode is most likely for autonomous and not as our drivetrain, which is why - // there is now a mecanumdrive op mode. - private GamepadEx driver_1; - private DriveSubsystem driveSubsystem; - private DefaultDrive defaultDrive; - - private MecanumDrive mecanumDrive; - - @Override - public void initialize() { - // TODO: we eventually want to add a second controller - // for the arm and a button to switch bewteen field centric and robot centric steering - // for this, however, we need to have the IMU and driver_2 control, we should do this - // AFTER we confirm the robot is working normally - - telemetry.addData("elapsedTime", 1); - telemetry.update(); - - // We initialize all of our hardware and controllers - driver_1 = new GamepadEx(gamepad1); - - mecanumDrive = new MecanumDrive( - new Motor(hardwareMap, "frontLeft"), - new Motor(hardwareMap, "frontRight"), - new Motor(hardwareMap, "backLeft"), - new Motor(hardwareMap, "backRight") - ); - - driveSubsystem = new DriveSubsystem(mecanumDrive); - - // We define our commands here - // basic robot-centric movement command - defaultDrive = new DefaultDrive(driveSubsystem, - driver_1.getLeftX(), //this is kinda wierd and might be a source of errors - driver_1.getLeftY(), - driver_1.getRightX() - ); - - // once we register our drivesystem, we can assign it a default command so that - // it gets run by the command scheduler - // https://docs.ftclib.org/ftclib/command-base/command-system/command-scheduler#step-4-schedule-default-commands - - run(); - driveSubsystem.setDefaultCommand(new DefaultDrive(driveSubsystem, - driver_1.getLeftX(), //this is kinda wierd and might be a source of errors - driver_1.getLeftY(), - driver_1.getRightX() - )); - register(driveSubsystem); - - - schedule(new RunCommand(telemetry::update)); - } - - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LilPrince.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LilPrince.java index 5ee93087430c..0aa0ee7673b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LilPrince.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LilPrince.java @@ -5,9 +5,9 @@ import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.hardware.motors.Motor; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.Gamepad; -import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSub; +import org.firstinspires.ftc.teamcode.commands.DriveCommand; +import org.firstinspires.ftc.teamcode.subsystems.Mecanum; public class LilPrince extends Robot { @@ -17,7 +17,10 @@ public class LilPrince extends Robot { public GamepadEx player2; // SUBSYSTEMS - public MecanumDriveSub drive; + public Mecanum drive; + + + /** * Welcome to the Command pattern. Here we assemble the robot and kick-off the command @@ -39,10 +42,24 @@ public LilPrince(LinearOpMode opMode) { * Set teleOp's default commands and player control bindings */ public void initTele() { - drive = new MecanumDriveSub(new MecanumDrive(new Motor(opMode.hardwareMap, "frontLeft"), + + drive = new Mecanum(new MecanumDrive(new Motor(opMode.hardwareMap, "frontLeft"), new Motor(opMode.hardwareMap, "frontRight"), new Motor(opMode.hardwareMap, "backLeft"), new Motor(opMode.hardwareMap, "backRight"))); + opMode.telemetry.addData("Mecanum Check:", true); + opMode.telemetry.update(); + register(drive); + + drive.setDefaultCommand(new DriveCommand(drive, + player1.getLeftX(), //this is kinda wierd and might be a source of errors + player1.getLeftY(), + player1.getRightX() + )); + + opMode.telemetry.addData("Command Check:", true); + opMode.telemetry.update(); + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/DriveCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/DriveCommand.java index 679c74e6a2bf..d1a9a3f0d80b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/DriveCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/DriveCommand.java @@ -1,18 +1,24 @@ package org.firstinspires.ftc.teamcode.commands; import com.arcrobotics.ftclib.command.CommandBase; +import com.arcrobotics.ftclib.gamepad.GamepadEx; -import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSub; +import org.firstinspires.ftc.teamcode.LilPrince; +import org.firstinspires.ftc.teamcode.subsystems.Mecanum; public class DriveCommand extends CommandBase { - private final MecanumDriveSub mc_d; + private final Mecanum drive; + private GamepadEx player1; - private final double strafeSpeed; - private final double forwardSpeed; - private final double turnSpeed; - public DriveCommand(MecanumDriveSub driveSubsystem, double strafeSpeed, double forwardSpeed, double turnSpeed) { - this.mc_d = driveSubsystem; + + + private double strafeSpeed; + private double forwardSpeed; + private double turnSpeed; + + public DriveCommand(Mecanum driveSubsystem, double strafeSpeed, double forwardSpeed, double turnSpeed) { + this.drive = driveSubsystem; this.strafeSpeed = strafeSpeed; this.forwardSpeed = forwardSpeed; this.turnSpeed = turnSpeed; @@ -27,7 +33,12 @@ public void initialize(){} @Override public void execute() { - mc_d.driveRobot(strafeSpeed, forwardSpeed, turnSpeed); + // Check control input + strafeSpeed = player1.getLeftX(); + forwardSpeed = player1.getLeftY(); + turnSpeed = player1.getRightX(); + + drive.driveRobot(strafeSpeed, forwardSpeed, turnSpeed); } public boolean isFinished() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DefaultDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DefaultDrive.java deleted file mode 100644 index 17d04944652e..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DefaultDrive.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import com.arcrobotics.ftclib.command.CommandBase; - -public class DefaultDrive extends CommandBase { - private final DriveSubsystem driveSubsystem; - - private final double strafeSpeed; - private final double forwardSpeed; - private final double turnSpeed; - - public DefaultDrive(DriveSubsystem driveSubsystem, double strafeSpeed, double forwardSpeed, double turnSpeed) { - this.driveSubsystem = driveSubsystem; - this.strafeSpeed = strafeSpeed; - this.forwardSpeed = forwardSpeed; - this.turnSpeed = turnSpeed; - addRequirements(driveSubsystem); - } - - // We call the command in our subsystem class to move our robot - // this for know is not taking robt centric vs field centric into consideration - // so we might need to rethink this - @Override - public void initialize(){} - - @Override - public void execute() { - driveSubsystem.driveRobot(strafeSpeed, forwardSpeed, turnSpeed); - } - - public boolean isFinished() { - // currently just a placeholder - return false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveSubsystem.java deleted file mode 100644 index 2a92c34e9b34..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveSubsystem.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import com.arcrobotics.ftclib.command.CommandScheduler; -import com.arcrobotics.ftclib.command.SubsystemBase; -import com.arcrobotics.ftclib.drivebase.MecanumDrive; - -public class DriveSubsystem extends SubsystemBase { - private final MecanumDrive mecanumDrive; - - public DriveSubsystem(MecanumDrive mecanumDrive) { - this.mecanumDrive = mecanumDrive; - } - - // Our default drive system that is robot centric - // We should add field centric once we implement IMU - public void driveRobot(double strafeSpeed, double forwardSpeed, double turnSpeed) { - mecanumDrive.driveRobotCentric(strafeSpeed, forwardSpeed, turnSpeed); - } - - @Override - public void periodic() { - super.periodic(); - CommandScheduler.getInstance().run(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSub.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Mecanum.java similarity index 51% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSub.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Mecanum.java index 6523277e730b..aafac2759ca6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSub.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Mecanum.java @@ -1,17 +1,21 @@ package org.firstinspires.ftc.teamcode.subsystems; import com.arcrobotics.ftclib.command.CommandScheduler; -import com.arcrobotics.ftclib.command.Robot; import com.arcrobotics.ftclib.command.SubsystemBase; import com.arcrobotics.ftclib.drivebase.MecanumDrive; +import com.arcrobotics.ftclib.hardware.motors.Motor; +import com.arcrobotics.ftclib.hardware.motors.MotorEx; +import com.qualcomm.robotcore.hardware.DcMotor; -public class MecanumDriveSub extends SubsystemBase { +import org.firstinspires.ftc.teamcode.LilPrince; - private final MecanumDrive drive; +public class Mecanum extends SubsystemBase { - public MecanumDriveSub (MecanumDrive mecanumDrive) { - drive = mecanumDrive; - } + // SUBSYSTEM ASSETS + private final LilPrince m_robot; + // MOTORS + private final MotorEx leftFront, leftBack, rightFront, rightBack; + //TODO: we need to get this subsystem working and give it the tasks. // Our default drive system that is robot centric // We should add field centric once we implement IMU @@ -19,9 +23,5 @@ public void driveRobot(double strafeSpeed, double forwardSpeed, double turnSpeed drive.driveRobotCentric(strafeSpeed, forwardSpeed, turnSpeed); } - @Override - public void periodic() { - super.periodic(); - CommandScheduler.getInstance().run(); - } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DriveyMcDriverson.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DriveyMcDriverson.java index 1e0cf56b104a..f4c2e3554d61 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DriveyMcDriverson.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DriveyMcDriverson.java @@ -7,11 +7,17 @@ import org.firstinspires.ftc.teamcode.LilPrince; -@TeleOp(name="TeleOp - Primary") +@TeleOp(name="TeleOp - Main") public class DriveyMcDriverson extends CommandOpMode { @Override public void initialize() { + + + boolean isWorking = true; + telemetry.addData("Is it working: ", isWorking); + telemetry.update(); + Robot m_robot = new LilPrince(this); /* We build our robot. From here on out, we don't need this file. When we build the robot,