From 9daeabc657c7a0545a988e3f726de198b7bd64ba Mon Sep 17 00:00:00 2001 From: Titan Robotics Club Date: Sun, 21 Jul 2024 01:58:42 -0700 Subject: [PATCH] Added Seafair demo robots. --- .../teamcode/seafairdemo/CommonTeleOp.java | 148 ++++++++++++++++++ .../seafairdemo/FtcDifferentialTeleOp.java | 47 ++++++ .../seafairdemo/FtcMecanumTeleOp.java | 47 ++++++ .../main/java/teamcode/seafairdemo/Robot.java | 148 ++++++++++++++++++ 4 files changed, 390 insertions(+) create mode 100644 TeamCode/src/main/java/teamcode/seafairdemo/CommonTeleOp.java create mode 100644 TeamCode/src/main/java/teamcode/seafairdemo/FtcDifferentialTeleOp.java create mode 100644 TeamCode/src/main/java/teamcode/seafairdemo/FtcMecanumTeleOp.java create mode 100644 TeamCode/src/main/java/teamcode/seafairdemo/Robot.java diff --git a/TeamCode/src/main/java/teamcode/seafairdemo/CommonTeleOp.java b/TeamCode/src/main/java/teamcode/seafairdemo/CommonTeleOp.java new file mode 100644 index 0000000..d383f0b --- /dev/null +++ b/TeamCode/src/main/java/teamcode/seafairdemo/CommonTeleOp.java @@ -0,0 +1,148 @@ +/* + * Copyright (c) 2024 Titan Robotics Club (http://www.titanrobotics.com) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package teamcode.seafairdemo; + +import ftclib.driverio.FtcGamepad; +import ftclib.robotcore.FtcOpMode; +import trclib.driverio.TrcGameController; + +/** + * This class contains the TeleOp Mode program. + */ +public abstract class CommonTeleOp extends FtcOpMode +{ + private Robot robot; + private FtcGamepad driverGamepad; + private boolean manualOverride = false; + + /** + * This method is called to initialize the robot. In FTC, this is called when the "Init" button on the Driver + * Station is pressed. + * + * @param roverRuckusRobot specifies true for the Rover Ruckus drive base, false for differential drive base. + */ + public void teleOpInit(boolean roverRuckusRobot) + { + robot = new Robot(roverRuckusRobot); + driverGamepad = new FtcGamepad("DriverGamepad", gamepad1, this::driverButtonEvent); + driverGamepad.setYInverted(true); + } //teleOpInit + + // + // Overrides TrcRobot.RobotMode methods. + // + + /** + * This method is called periodically on the main robot thread. Typically, you put TeleOp control code here that + * doesn't require frequent update For example, TeleOp joystick code or status display code can be put here since + * human responses are considered slow. + * + * @param elapsedTime specifies the elapsed time since the mode started. + * @param slowPeriodicLoop specifies true if it is running the slow periodic loop on the main robot thread, + * false otherwise. + */ + @Override + public void periodic(double elapsedTime, boolean slowPeriodicLoop) + { + if (slowPeriodicLoop) + { + int lineNum = 1; + // + // DriveBase subsystem. + // + double[] inputs = driverGamepad.getDriveInputs( + FtcGamepad.DriveMode.ARCADE_MODE, true, Robot.DRIVE_POWER_SCALE, Robot.TURN_POWER_SCALE); + if (robot.driveBase.supportsHolonomicDrive()) + { + robot.driveBase.holonomicDrive(inputs[0], inputs[1], inputs[2]); + } + else + { + robot.driveBase.arcadeDrive(inputs[1], inputs[2]); + } + robot.dashboard.displayPrintf( + lineNum++, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f)", inputs[0], inputs[1], inputs[2]); + // + // Other subsystems. + // + if (robot.elevator != null) + { + double elevatorPower = driverGamepad.getRightTrigger(true) - driverGamepad.getLeftTrigger(true); + if (manualOverride) + { + robot.setElevatorPower(elevatorPower); + } + else + { + robot.setElevatorPidPower(elevatorPower); + } + robot.dashboard.displayPrintf( + lineNum++, "Elevator: power=%.1f, pos=%.1f/%.1f, limitSw=%s/%s", + robot.elevator.getPower(), robot.elevator.getPosition(), robot.elevator.getPidTarget(), + robot.elevator.isLowerLimitSwitchActive(), robot.elevator.isUpperLimitSwitchActive()); + } + } + } //periodic + + // + // Implements TrcGameController.ButtonHandler interface. + // + + /** + * This method is called when driver gamepad button event is detected. + * + * @param gamepad specifies the game controller object that generated the event. + * @param button specifies the button ID that generates the event. + * @param pressed specifies true if the button is pressed, false otherwise. + */ + public void driverButtonEvent(TrcGameController gamepad, int button, boolean pressed) + { + robot.dashboard.displayPrintf(8, "%s: %04x->%s", gamepad, button, pressed? "Pressed": "Released"); + + switch (button) + { + case FtcGamepad.GAMEPAD_A: + case FtcGamepad.GAMEPAD_B: + case FtcGamepad.GAMEPAD_X: + case FtcGamepad.GAMEPAD_Y: + case FtcGamepad.GAMEPAD_LBUMPER: + break; + + case FtcGamepad.GAMEPAD_RBUMPER: +// manualOverride = pressed; + break; + + case FtcGamepad.GAMEPAD_DPAD_UP: + case FtcGamepad.GAMEPAD_DPAD_DOWN: + case FtcGamepad.GAMEPAD_DPAD_LEFT: + case FtcGamepad.GAMEPAD_DPAD_RIGHT: + case FtcGamepad.GAMEPAD_START: + break; + + case FtcGamepad.GAMEPAD_BACK: + robot.zeroCalibrate(); + break; + } + } //driverButtonEvent + +} //class CommonTeleOp diff --git a/TeamCode/src/main/java/teamcode/seafairdemo/FtcDifferentialTeleOp.java b/TeamCode/src/main/java/teamcode/seafairdemo/FtcDifferentialTeleOp.java new file mode 100644 index 0000000..753daa2 --- /dev/null +++ b/TeamCode/src/main/java/teamcode/seafairdemo/FtcDifferentialTeleOp.java @@ -0,0 +1,47 @@ +/* + * Copyright (c) 2024 Titan Robotics Club (http://www.titanrobotics.com) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package teamcode.seafairdemo; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/** + * This class contains the TeleOp Mode program for the Differential DriveBase. + */ +@TeleOp(name="FtcDifferentialTeleOp", group="SeafairDemo") +public class FtcDifferentialTeleOp extends CommonTeleOp +{ + // + // Implements FtcOpMode abstract method. + // + + /** + * This method is called to initialize the robot. In FTC, this is called when the "Init" button on the Driver + * Station is pressed. + */ + @Override + public void robotInit() + { + super.teleOpInit(false); + } //robotInit + +} //class FtcDifferentialTeleOp diff --git a/TeamCode/src/main/java/teamcode/seafairdemo/FtcMecanumTeleOp.java b/TeamCode/src/main/java/teamcode/seafairdemo/FtcMecanumTeleOp.java new file mode 100644 index 0000000..c90e487 --- /dev/null +++ b/TeamCode/src/main/java/teamcode/seafairdemo/FtcMecanumTeleOp.java @@ -0,0 +1,47 @@ +/* + * Copyright (c) 2024 Titan Robotics Club (http://www.titanrobotics.com) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package teamcode.seafairdemo; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/** + * This class contains the TeleOp Mode program for the Mecanum DriveBase. + */ +@TeleOp(name="FtcMecanumTeleOp", group="SeafairDemo") +public class FtcMecanumTeleOp extends CommonTeleOp +{ + // + // Implements FtcOpMode abstract method. + // + + /** + * This method is called to initialize the robot. In FTC, this is called when the "Init" button on the Driver + * Station is pressed. + */ + @Override + public void robotInit() + { + super.teleOpInit(true); + } //robotInit + +} //class FtcMecanumTeleOp diff --git a/TeamCode/src/main/java/teamcode/seafairdemo/Robot.java b/TeamCode/src/main/java/teamcode/seafairdemo/Robot.java new file mode 100644 index 0000000..e0ea311 --- /dev/null +++ b/TeamCode/src/main/java/teamcode/seafairdemo/Robot.java @@ -0,0 +1,148 @@ +/* + * Copyright (c) 2024 Titan Robotics Club (http://www.titanrobotics.com) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package teamcode.seafairdemo; + +import ftclib.driverio.FtcDashboard; +import ftclib.motor.FtcDcMotor; +import ftclib.subsystem.FtcMotorActuator; +import trclib.dataprocessor.TrcUtil; +import trclib.drivebase.TrcDriveBase; +import trclib.drivebase.TrcMecanumDriveBase; +import trclib.drivebase.TrcSimpleDriveBase; +import trclib.motor.TrcMotor; + +/** + * This class creates the robot object that consists of sensors, indicators, drive base and all the subsystems. + */ +public class Robot +{ + // DriveBase Subsystem + private static final int INDEX_LEFT_FRONT = 0; + private static final int INDEX_RIGHT_FRONT = 1; + private static final int INDEX_LEFT_BACK = 2; + private static final int INDEX_RIGHT_BACK = 3; + private static final String[] driveMotorNames = {"lfDriveWheel", "rfDriveWheel", "lbDriveWheel", "rbDriveWheel"}; + private static final boolean[] driveMotorInverted = {true, false, true, false}; + public static final double DRIVE_POWER_SCALE = 0.5; + public static final double TURN_POWER_SCALE = 0.5; + // Elevator Subsystem + private static final String ELEVATOR_NAME = "elevator"; + private static final boolean ELEVATOR_MOTOR_INVERTED = false; + private static final boolean ELEVATOR_HAS_LOWER_LIMIT_SWITCH = true; + private static final boolean ELEVATOR_LOWER_LIMIT_SWITCH_INVERTED = true; + private static final boolean ELEVATOR_HAS_UPPER_LIMIT_SWITCH = true; + private static final boolean ELEVATOR_UPPER_LIMIT_SWITCH_INVERTED = true; + private static final double ELEVATOR_INCHES_PER_COUNT = 5.625/8498.0; + private static final double ELEVATOR_OFFSET = 16.25; + private static final double ELEVATOR_TOLERANCE = 0.2; + private static final double ELEVATOR_CAL_POWER = -0.3; + private static final double ELEVATOR_MIN_HEIGHT = ELEVATOR_OFFSET - 0.1; + private static final double ELEVATOR_MAX_HEIGHT = 23.0; + + public final FtcDashboard dashboard; + public final TrcDriveBase driveBase; + public final TrcMotor elevator; + + /** + * Constructor: Create an instance of the object. + * + * @param roverRuckusRobot specifies true for the Rover Ruckus drive base, false for differential drive base. + */ + public Robot(boolean roverRuckusRobot) + { + dashboard = FtcDashboard.getInstance(); + + FtcDcMotor[] driveMotors = new FtcDcMotor[driveMotorNames.length]; + for (int i = 0; i < driveMotorNames.length; i++) + { + driveMotors[i] = new FtcDcMotor(driveMotorNames[i]); + driveMotors[i].setMotorInverted(driveMotorInverted[i]); + driveMotors[i].setBrakeModeEnabled(true); + driveMotors[i].setVoltageCompensationEnabled(TrcUtil.BATTERY_NOMINAL_VOLTAGE); + } + + if (roverRuckusRobot) + { + // Rover Ruckus robot has a mecanum drive base and an elevator with lower and upper limit switches. + driveBase = new TrcMecanumDriveBase( + driveMotors[INDEX_LEFT_FRONT], driveMotors[INDEX_LEFT_BACK], + driveMotors[INDEX_RIGHT_FRONT], driveMotors[INDEX_RIGHT_BACK]); + driveBase.setDriveOrientation(TrcDriveBase.DriveOrientation.ROBOT, false); + FtcMotorActuator.Params elevatorParams = new FtcMotorActuator.Params() + .setMotorInverted(ELEVATOR_MOTOR_INVERTED) + .setLowerLimitSwitch(ELEVATOR_HAS_LOWER_LIMIT_SWITCH, ELEVATOR_LOWER_LIMIT_SWITCH_INVERTED) + .setUpperLimitSwitch(ELEVATOR_HAS_UPPER_LIMIT_SWITCH, ELEVATOR_UPPER_LIMIT_SWITCH_INVERTED) + .setVoltageCompensationEnabled(true) + .setPositionScaleAndOffset(ELEVATOR_INCHES_PER_COUNT, ELEVATOR_OFFSET); + elevator = new FtcMotorActuator(ELEVATOR_NAME, elevatorParams).getActuator(); + elevator.setBrakeModeEnabled(true); + elevator.setPositionPidTolerance(ELEVATOR_TOLERANCE); + } + else + { + // Differential drive base is a West Coast drive base with 6 wheels and four drive motors. + driveBase = new TrcSimpleDriveBase( + driveMotors[INDEX_LEFT_FRONT], driveMotors[INDEX_LEFT_BACK], + driveMotors[INDEX_RIGHT_FRONT], driveMotors[INDEX_RIGHT_BACK]); + elevator = null; + } + } //Robot + + /** + * This method zero calibrates all subsystems. + */ + public void zeroCalibrate() + { + if (elevator != null) + { + elevator.zeroCalibrate(ELEVATOR_CAL_POWER); + } + } //zeroCalibrate + + /** + * This method sets the elevator raw power when manual override is ON. + * + * @param power specifies elevator power applied. + */ + public void setElevatorPower(double power) + { + if (elevator != null) + { + elevator.setPower(power); + } + } //setElevatorPower + + /** + * This method sets the elevator pid power using PID control and will slow down when approaching limits. + * + * @param power specifies elevator power applied. + */ + public void setElevatorPidPower(double power) + { + if (elevator != null) + { + elevator.setPidPower(power, ELEVATOR_MIN_HEIGHT, ELEVATOR_MAX_HEIGHT, true); + } + } //setElevatorPidPower + +} //class Robot