SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-01 13:01:24 -07:00
Code Issues Packages Projects Releases Wiki Activity

Added Seafair demo bots.

Browse Source
This commit is contained in:
Titan Robotics Club
2024-07-21 03:27:44 -07:00
parent 8243d67d4e
commit 1326226648
4 changed files with 390 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
TeamCode/src/main/java/teamcode/seafairdemo/CommonTeleOp.java Normal file
View File

@ -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 Common TeleOp Mode code and is extended by different TeleOp OpModes.
*/
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

47
TeamCode/src/main/java/teamcode/seafairdemo/FtcDifferentialTeleOp.java Normal file
View File

@ -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

47
TeamCode/src/main/java/teamcode/seafairdemo/FtcMecanumTeleOp.java Normal file
View File

@ -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

148
TeamCode/src/main/java/teamcode/seafairdemo/Robot.java Normal file
View File

@ -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