SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-04 06:21:23 -07:00
Code Issues Packages Projects Releases Wiki Activity

Removed the fork relationship from FTC SDK so that people can fork this repo without problem if they already had an FTC SDK fork.

Browse Source
This commit is contained in:
Titan Robotics Club
2024-05-28 17:44:09 -07:00
parent 34a9ce290b
commit 653736be1c
145 changed files with 602787 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -0,0 +1,323 @@
/*
* Copyright (c) 2022 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;
import TrcCommonLib.trclib.TrcDbgTrace;
import TrcCommonLib.trclib.TrcDigitalInput;
import TrcCommonLib.trclib.TrcMotor;
import TrcCommonLib.trclib.TrcPose2D;
import TrcCommonLib.trclib.TrcRobot;
import TrcCommonLib.trclib.TrcServo;
import TrcCommonLib.trclib.TrcTimer;
import TrcFtcLib.ftclib.FtcDashboard;
import TrcFtcLib.ftclib.FtcMatchInfo;
import TrcFtcLib.ftclib.FtcOpMode;
import TrcFtcLib.ftclib.FtcRobotBattery;
import teamcode.drivebases.MecanumDrive;
import teamcode.drivebases.RobotDrive;
import teamcode.drivebases.SwerveDrive;
import teamcode.subsystems.BlinkinLEDs;
import teamcode.vision.Vision;
/**
* This class creates the robot object that consists of sensors, indicators, drive base and all the subsystems.
*/
public class Robot
{
private static final String moduleName = Robot.class.getSimpleName();
private static final double STATUS_UPDATE_INTERVAL = 0.1; // 100 msec
//
// Global objects.
//
public final FtcOpMode opMode;
public final TrcDbgTrace globalTracer;
public final FtcDashboard dashboard;
public static FtcMatchInfo matchInfo = null;
private static TrcPose2D endOfAutoRobotPose = null;
private static double nextStatusUpdateTime = 0.0;
//
// Vision subsystems.
//
public Vision vision;
//
// Sensors and indicators.
//
public BlinkinLEDs blinkin;
public FtcRobotBattery battery;
//
// Subsystems.
//
public RobotDrive robotDrive;
/**
* Constructor: Create an instance of the object.
*
* @param runMode specifies robot running mode (Auto, TeleOp, Test), can be used to create and initialize mode
* specific sensors and subsystems if necessary.
*/
public Robot(TrcRobot.RunMode runMode)
{
//
// Initialize global objects.
//
opMode = FtcOpMode.getInstance();
globalTracer = TrcDbgTrace.getGlobalTracer();
dashboard = FtcDashboard.getInstance();
nextStatusUpdateTime = TrcTimer.getCurrentTime();
speak("Init starting");
//
// Initialize vision subsystems.
//
if (RobotParams.Preferences.tuneColorBlobVision ||
RobotParams.Preferences.useAprilTagVision ||
RobotParams.Preferences.useColorBlobVision ||
RobotParams.Preferences.useTensorFlowVision)
{
vision = new Vision(this);
}
//
// If robotType is NoRobot, the robot controller is disconnected from the robot for testing vision.
// In this case, we should not instantiate any robot hardware.
//
if (RobotParams.Preferences.robotType != RobotParams.RobotType.NoRobot)
{
//
// Create and initialize sensors and indicators.
//
if (RobotParams.Preferences.useBlinkin)
{
blinkin = new BlinkinLEDs(RobotParams.HWNAME_BLINKIN);
}
if (RobotParams.Preferences.useBatteryMonitor)
{
battery = new FtcRobotBattery();
}
//
// Create and initialize RobotDrive.
//
robotDrive =
RobotParams.Preferences.robotType == RobotParams.RobotType.SwerveRobot?
new SwerveDrive(): new MecanumDrive();
//
// Create and initialize other subsystems.
//
if (RobotParams.Preferences.useSubsystems)
{
}
}
speak("Init complete");
} //Robot
/**
* This method returns the instance name.
*
* @return instance name.
*/
@Override
public String toString()
{
return RobotParams.ROBOT_NAME;
} //toString
/**
* This method is call when the robot mode is about to start. It contains code to initialize robot hardware
* necessary for running the robot mode.
*
* @param runMode specifies the robot mode it is about to start, can be used to initialize mode specific hardware.
*/
public void startMode(TrcRobot.RunMode runMode)
{
if (robotDrive != null)
{
//
// Since the IMU gyro is giving us cardinal heading, we need to enable its cardinal to cartesian converter.
//
if (robotDrive.gyro != null)
{
robotDrive.gyro.setEnabled(true);
// The following are performance counters, could be disabled for competition if you want.
// But it might give you some insight if somehow autonomous wasn't performing as expected.
robotDrive.gyro.setElapsedTimerEnabled(true);
}
//
// Enable odometry for all opmodes. We may need odometry in TeleOp for auto-assist drive.
//
robotDrive.driveBase.setOdometryEnabled(true);
if (runMode == TrcRobot.RunMode.TELEOP_MODE)
{
if (endOfAutoRobotPose != null)
{
// We had a previous autonomous run that saved the robot position at the end, use it.
robotDrive.driveBase.setFieldPosition(endOfAutoRobotPose);
globalTracer.traceInfo(moduleName, "Restore saved RobotPose=" + endOfAutoRobotPose);
}
}
// Consume it so it's no longer valid for next run.
endOfAutoRobotPose = null;
}
TrcDigitalInput.setElapsedTimerEnabled(true);
TrcMotor.setElapsedTimerEnabled(true);
TrcServo.setElapsedTimerEnabled(true);
} //startMode
/**
* This method is call when the robot mode is about to end. It contains code to cleanup robot hardware before
* exiting the robot mode.
*
* @param runMode specifies the robot mode it is about to stop, can be used to cleanup mode specific hardware.
*/
public void stopMode(TrcRobot.RunMode runMode)
{
//
// Print all performance counters if there are any.
//
if (robotDrive != null && robotDrive.gyro != null)
{
robotDrive.gyro.printElapsedTime(globalTracer);
robotDrive.gyro.setElapsedTimerEnabled(false);
}
TrcDigitalInput.printElapsedTime(globalTracer);
TrcDigitalInput.setElapsedTimerEnabled(false);
TrcMotor.printElapsedTime(globalTracer);
TrcMotor.setElapsedTimerEnabled(false);
TrcServo.printElapsedTime(globalTracer);
TrcServo.setElapsedTimerEnabled(false);
//
// Disable vision.
//
if (vision != null)
{
if (vision.rawColorBlobVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling RawColorBlobVision.");
vision.setRawColorBlobVisionEnabled(false);
}
if (vision.aprilTagVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling AprilTagVision.");
vision.setAprilTagVisionEnabled(false);
}
if (vision.redBlobVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling RedBlobVision.");
vision.setRedBlobVisionEnabled(false);
}
if (vision.blueBlobVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling BlueBlobVision.");
vision.setBlueBlobVisionEnabled(false);
}
if (vision.tensorFlowVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling TensorFlowVision.");
vision.setTensorFlowVisionEnabled(false);
}
vision.close();
}
if (robotDrive != null)
{
if (runMode == TrcRobot.RunMode.AUTO_MODE)
{
// Save current robot location at the end of autonomous so subsequent teleop run can restore it.
endOfAutoRobotPose = robotDrive.driveBase.getFieldPosition();
globalTracer.traceInfo(moduleName, "Saved robot pose=" + endOfAutoRobotPose);
}
//
// Disable odometry.
//
robotDrive.driveBase.setOdometryEnabled(false);
//
// Disable gyro task.
//
if (robotDrive.gyro != null)
{
robotDrive.gyro.setEnabled(false);
}
}
} //stopMode
/**
* This method update all subsystem status on the dashboard.
*/
public void updateStatus()
{
if (TrcTimer.getCurrentTime() > nextStatusUpdateTime)
{
int lineNum = 2;
nextStatusUpdateTime += STATUS_UPDATE_INTERVAL;
if (robotDrive != null)
{
dashboard.displayPrintf(lineNum++, "DriveBase: Pose=%s", robotDrive.driveBase.getFieldPosition());
}
//
// Display other subsystem status here.
//
}
} //updateStatus
/**
* This method zero calibrates all subsystems.
*
* @param owner specifies the owner ID to check if the caller has ownership of the motor.
*/
public void zeroCalibrate(String owner)
{
} //zeroCalibrate
/**
* This method zero calibrates all subsystems.
*/
public void zeroCalibrate()
{
zeroCalibrate(null);
} //zeroCalibrate
/**
* This method sets the robot's starting position according to the autonomous choices.
*
* @param autoChoices specifies all the auto choices.
*/
public void setRobotStartPosition(FtcAuto.AutoChoices autoChoices)
{
} //setRobotStartPosition
/**
* This method sends the text string to the Driver Station to be spoken using text to speech.
*
* @param sentence specifies the sentence to be spoken by the Driver Station.
*/
public void speak(String sentence)
{
opMode.telemetry.speak(sentence);
} //speak
} //class Robot