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

Sync'd FRC changes.

Browse Source
This commit is contained in:
Titan Robotics Club
2025-04-26 13:23:51 -07:00
parent 7854a766a2
commit d4f39a1346
2 changed files with 145 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
TeamCode/src/main/java/teamcode/Dashboard.java
View File

@ -82,6 +82,12 @@ public class Dashboard
@Config @Config
public static class Drive public static class Drive
{ {
public static double xTarget = 0.0;
public static double yTarget = 0.0;
public static double turnTarget = 0.0;
public static double drivePower = 1.0;
public static double turnPower = 1.0;
public static double driveTime = 0.0;
public static TrcPidController.PidCoefficients xPidCoeffs = public static TrcPidController.PidCoefficients xPidCoeffs =
new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0); new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0);
public static TrcPidController.PidCoefficients yPidCoeffs = public static TrcPidController.PidCoefficients yPidCoeffs =
@ -90,13 +96,7 @@ public class Dashboard
new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0); new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0);
public static double maxVelocity = 0.0; public static double maxVelocity = 0.0;
public static double maxAcceleration = 0.0; public static double maxAcceleration = 0.0;
public static double maxDeclearion = 0.0; public static double maxDeceleration = 0.0;
public static double xTarget = 0.0;
public static double yTarget = 0.0;
public static double turnTarget = 0.0;
public static double drivePower = 1.0;
public static double turnPower = 1.0;
public static double driveTime = 0.0;
} //class Drive } //class Drive
@Config @Config

264
TeamCode/src/main/java/teamcode/FtcTest.java
View File

@ -99,7 +99,7 @@ public class FtcTest extends FtcTeleOp
private double maxTurnVelocity = 0.0; private double maxTurnVelocity = 0.0;
private double prevTime = 0.0; private double prevTime = 0.0;
private double prevVelocity = 0.0; private double prevVelocity = 0.0;
// Tune Drive. // Tune Drive PID.
private TrcPose2D tuneDriveStartPoint; private TrcPose2D tuneDriveStartPoint;
private TrcPose2D tuneDriveEndPoint; private TrcPose2D tuneDriveEndPoint;
private boolean tuneDriveAtEndPoint; private boolean tuneDriveAtEndPoint;
@ -131,44 +131,6 @@ public class FtcTest extends FtcTeleOp
// Test menus. // Test menus.
// //
doTestMenus(); doTestMenus();
//
// Create the robot command for the tests that need one.
//
switch (testChoices.test)
{
case DRIVE_MOTORS_TEST:
if (robot.robotDrive != null)
{
testCommand = new CmdDriveMotorsTest(
robot.robotDrive.driveBase, robot.robotDrive.driveMotors, 5.0, 0.5);
}
break;
case X_TIMED_DRIVE:
if (robot.robotDrive != null)
{
testCommand = new CmdTimedDrive(
robot.robotDrive.driveBase, 0.0, Dashboard.Drive.driveTime,
Dashboard.Drive.drivePower, 0.0, 0.0);
}
break;
case Y_TIMED_DRIVE:
if (robot.robotDrive != null)
{
testCommand = new CmdTimedDrive(
robot.robotDrive.driveBase, 0.0, Dashboard.Drive.driveTime,
0.0, Dashboard.Drive.drivePower, 0.0);
}
break;
case PID_DRIVE:
if (robot.robotDrive != null)
{
testCommand = new CmdPidDrive(robot.robotDrive.driveBase, robot.robotDrive.pidDrive);
}
break;
}
} //robotInit } //robotInit
// //
@ -230,9 +192,39 @@ public class FtcTest extends FtcTeleOp
} }
break; break;
case PID_DRIVE: case DRIVE_MOTORS_TEST:
if (testCommand != null) if (robot.robotDrive != null)
{ {
testCommand = new CmdDriveMotorsTest(
robot.robotDrive.driveBase, robot.robotDrive.driveMotors, 5.0, 0.5);
}
break;
case X_TIMED_DRIVE:
if (robot.robotDrive != null && robot.robotDrive.driveBase.supportsHolonomicDrive())
{
robot.robotDrive.driveBase.resetOdometry();
testCommand = new CmdTimedDrive(
robot.robotDrive.driveBase, 0.0, Dashboard.Drive.driveTime,
Dashboard.Drive.drivePower, 0.0, 0.0);
}
break;
case Y_TIMED_DRIVE:
if (robot.robotDrive != null)
{
robot.robotDrive.driveBase.resetOdometry();
testCommand = new CmdTimedDrive(
robot.robotDrive.driveBase, 0.0, Dashboard.Drive.driveTime,
0.0, Dashboard.Drive.drivePower, 0.0);
}
break;
case PID_DRIVE:
if (robot.robotDrive != null && robot.robotDrive.pidDrive != null)
{
robot.robotDrive.driveBase.resetOdometry();
testCommand = new CmdPidDrive(robot.robotDrive.driveBase, robot.robotDrive.pidDrive);
((CmdPidDrive) testCommand).start( ((CmdPidDrive) testCommand).start(
0.0, Dashboard.Drive.drivePower, null, 0.0, Dashboard.Drive.drivePower, null,
new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0, new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0,
@ -242,21 +234,25 @@ public class FtcTest extends FtcTeleOp
break; break;
case PP_DRIVE: case PP_DRIVE:
if (robot.robotDrive != null) if (robot.robotDrive != null && robot.robotDrive.purePursuitDrive != null)
{ {
robot.robotDrive.purePursuitDrive.setTraceLevel( robot.robotDrive.driveBase.resetOdometry();
TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false); robot.robotDrive.purePursuitDrive.setMoveOutputLimit(Dashboard.Drive.drivePower);
robot.robotDrive.purePursuitDrive.setRotOutputLimit(Dashboard.Drive.turnPower);
robot.robotDrive.purePursuitDrive.start( robot.robotDrive.purePursuitDrive.start(
true, robot.robotInfo.profiledMaxVelocity, robot.robotInfo.profiledMaxAcceleration, true, robot.robotInfo.profiledMaxVelocity, robot.robotInfo.profiledMaxAcceleration,
robot.robotInfo.profiledMaxAcceleration, robot.robotInfo.profiledMaxAcceleration,
new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0, new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0,
Dashboard.Drive.turnTarget)); Dashboard.Drive.turnTarget));
robot.robotDrive.purePursuitDrive.setTraceLevel(
TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
} }
break; break;
case TUNE_DRIVE_PID: case TUNE_DRIVE_PID:
if (robot.robotDrive != null) if (robot.robotDrive != null && robot.robotDrive.purePursuitDrive != null)
{ {
robot.robotDrive.driveBase.resetOdometry();
tuneDriveStartPoint = robot.robotDrive.driveBase.getFieldPosition(); tuneDriveStartPoint = robot.robotDrive.driveBase.getFieldPosition();
tuneDriveEndPoint = tuneDriveStartPoint.addRelativePose( tuneDriveEndPoint = tuneDriveStartPoint.addRelativePose(
new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0, new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0,
@ -301,7 +297,7 @@ public class FtcTest extends FtcTeleOp
@Override @Override
public void periodic(double elapsedTime, boolean slowPeriodicLoop) public void periodic(double elapsedTime, boolean slowPeriodicLoop)
{ {
int lineNum = 9; int lineNum = 1;
// //
// Run the testCommand if any. // Run the testCommand if any.
// //
@ -312,60 +308,79 @@ public class FtcTest extends FtcTeleOp
// //
// Display test status. // Display test status.
// //
if (testChoices.test == Test.DRIVE_SPEED_TEST) switch (testChoices.test)
{ {
if (robot.robotDrive != null) case DRIVE_SPEED_TEST:
{ if (robot.robotDrive != null)
double currTime = TrcTimer.getCurrentTime();
TrcPose2D velPose = robot.robotDrive.driveBase.getFieldVelocity();
double velocity = TrcUtil.magnitude(velPose.x, velPose.y);
double acceleration = 0.0;
double deceleration = 0.0;
double deltaTime = currTime - prevTime;
if (prevTime != 0.0)
{ {
if (velocity > prevVelocity) double currTime = TrcTimer.getCurrentTime();
TrcPose2D velPose = robot.robotDrive.driveBase.getFieldVelocity();
double velocity = TrcUtil.magnitude(velPose.x, velPose.y);
double acceleration = 0.0;
double deceleration = 0.0;
double deltaTime = currTime - prevTime;
if (prevTime != 0.0)
{ {
acceleration = (velocity - prevVelocity)/deltaTime; if (velocity > prevVelocity)
{
acceleration = (velocity - prevVelocity)/deltaTime;
}
else
{
deceleration = (prevVelocity - velocity)/deltaTime;
}
} }
else
if (velocity > maxDriveVelocity)
{ {
deceleration = (prevVelocity - velocity)/deltaTime; maxDriveVelocity = velocity;
} }
}
if (velocity > maxDriveVelocity) if (acceleration > maxDriveAcceleration)
{
maxDriveAcceleration = acceleration;
}
if (deceleration > maxDriveDeceleration)
{
maxDriveDeceleration = deceleration;
}
if (velPose.angle > maxTurnVelocity)
{
maxTurnVelocity = velPose.angle;
}
prevTime = currTime;
prevVelocity = velocity;
robot.dashboard.displayPrintf(lineNum++, "Drive Vel: (%.1f/%.1f)", velocity, maxDriveVelocity);
robot.dashboard.displayPrintf(
lineNum++, "Drive Accel: (%.1f/%.1f)", acceleration, maxDriveAcceleration);
robot.dashboard.displayPrintf(
lineNum++, "Drive Decel: (%.1f/%.1f)", deceleration, maxDriveDeceleration);
robot.dashboard.displayPrintf(
lineNum++, "Turn Vel: (%.1f/%.1f)", velPose.angle, maxTurnVelocity);
}
break;
case TUNE_DRIVE_PID:
if (robot.robotDrive != null && robot.robotDrive.purePursuitDrive != null)
{ {
maxDriveVelocity = velocity; robot.dashboard.putObject(
"robotVelocity", robot.robotDrive.purePursuitDrive.getPathRobotVelocity());
robot.dashboard.putObject(
"targetVelocity", robot.robotDrive.purePursuitDrive.getPathTargetVelocity());
robot.dashboard.putObject(
"robotPosition", robot.robotDrive.purePursuitDrive.getPathRelativePosition());
robot.dashboard.putObject(
"targetPosition", robot.robotDrive.purePursuitDrive.getPathPositionTarget());
} }
break;
if (acceleration > maxDriveAcceleration) default:
{ break;
maxDriveAcceleration = acceleration;
}
if (deceleration > maxDriveDeceleration)
{
maxDriveDeceleration = deceleration;
}
if (velPose.angle > maxTurnVelocity)
{
maxTurnVelocity = velPose.angle;
}
prevTime = currTime;
prevVelocity = velocity;
robot.dashboard.displayPrintf(lineNum++, "Drive Vel: (%.1f/%.1f)", velocity, maxDriveVelocity);
robot.dashboard.displayPrintf(
lineNum++, "Drive Accel: (%.1f/%.1f)", acceleration, maxDriveAcceleration);
robot.dashboard.displayPrintf(
lineNum++, "Drive Decel: (%.1f/%.1f)", deceleration, maxDriveDeceleration);
robot.dashboard.displayPrintf(
lineNum++, "Turn Vel: (%.1f/%.1f)", velPose.angle, maxTurnVelocity);
}
} }
if (elapsedTimer != null) if (elapsedTimer != null)
@ -415,54 +430,44 @@ public class FtcTest extends FtcTeleOp
} }
break; break;
case PP_DRIVE:
case PID_DRIVE:
case TUNE_DRIVE_PID: case TUNE_DRIVE_PID:
if (robot.robotDrive != null) if (robot.robotDrive != null)
{ {
robot.dashboard.putObject( TrcPidController xPidCtrl = null, yPidCtrl = null, turnPidCtrl = null;
"robotVelocity", robot.robotDrive.purePursuitDrive.getPathRobotVelocity());
robot.dashboard.putObject( if (testChoices.test == Test.PID_DRIVE && robot.robotDrive.pidDrive != null)
"targetVelocity", robot.robotDrive.purePursuitDrive.getPathTargetVelocity());
robot.dashboard.putObject(
"robotPosition", robot.robotDrive.purePursuitDrive.getPathRelativePosition());
robot.dashboard.putObject(
"targetPosition", robot.robotDrive.purePursuitDrive.getPathPositionTarget());
}
//
// Intentionally falling through.
//
case PP_DRIVE:
case PID_DRIVE:
if (robot.robotDrive != null)
{
TrcPidController xPidCtrl, yPidCtrl, turnPidCtrl;
if (testChoices.test == Test.PP_DRIVE)
{
xPidCtrl = robot.robotDrive.purePursuitDrive.getXPosPidCtrl();
yPidCtrl = robot.robotDrive.purePursuitDrive.getYPosPidCtrl();
turnPidCtrl = robot.robotDrive.purePursuitDrive.getTurnPidCtrl();
}
else
{ {
xPidCtrl = robot.robotDrive.pidDrive.getXPidCtrl(); xPidCtrl = robot.robotDrive.pidDrive.getXPidCtrl();
yPidCtrl = robot.robotDrive.pidDrive.getYPidCtrl(); yPidCtrl = robot.robotDrive.pidDrive.getYPidCtrl();
turnPidCtrl = robot.robotDrive.pidDrive.getTurnPidCtrl(); turnPidCtrl = robot.robotDrive.pidDrive.getTurnPidCtrl();
} }
else if (robot.robotDrive.purePursuitDrive != null)
{
xPidCtrl = robot.robotDrive.purePursuitDrive.getXPosPidCtrl();
yPidCtrl = robot.robotDrive.purePursuitDrive.getYPosPidCtrl();
turnPidCtrl = robot.robotDrive.purePursuitDrive.getTurnPidCtrl();
}
robot.dashboard.displayPrintf( robot.dashboard.displayPrintf(
lineNum++, "RobotPose=%s,rawEnc=lf:%.0f,rf:%.0f,lb:%.0f,rb:%.0f", lineNum++, "RobotPose=%s", robot.robotDrive.driveBase.getFieldPosition());
robot.robotDrive.driveBase.getFieldPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_FRONT].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_FRONT].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_BACK].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_BACK].getPosition());
if (xPidCtrl != null) if (xPidCtrl != null)
{ {
xPidCtrl.displayPidInfo(lineNum); xPidCtrl.displayPidInfo(lineNum);
lineNum += 2; lineNum += 2;
} }
yPidCtrl.displayPidInfo(lineNum); if (yPidCtrl != null)
lineNum += 2; {
turnPidCtrl.displayPidInfo(lineNum); yPidCtrl.displayPidInfo(lineNum);
lineNum += 2;
}
if (turnPidCtrl != null)
{
turnPidCtrl.displayPidInfo(lineNum);
lineNum += 2;
}
} }
break; break;
@ -474,6 +479,9 @@ public class FtcTest extends FtcTeleOp
swerveDrive.displaySteerZeroCalibration(lineNum); swerveDrive.displaySteerZeroCalibration(lineNum);
} }
break; break;
default:
break;
} }
} }
} //periodic } //periodic
@ -510,8 +518,11 @@ public class FtcTest extends FtcTeleOp
robot.robotDrive.purePursuitDrive.setMoveOutputLimit(Dashboard.Drive.drivePower); robot.robotDrive.purePursuitDrive.setMoveOutputLimit(Dashboard.Drive.drivePower);
robot.robotDrive.purePursuitDrive.setRotOutputLimit(Dashboard.Drive.turnPower); robot.robotDrive.purePursuitDrive.setRotOutputLimit(Dashboard.Drive.turnPower);
robot.robotDrive.purePursuitDrive.start( robot.robotDrive.purePursuitDrive.start(
false, Dashboard.Drive.maxVelocity, Dashboard.Drive.maxAcceleration, false,
Dashboard.Drive.maxDeclearion, tuneDriveAtEndPoint? tuneDriveStartPoint: tuneDriveEndPoint); Dashboard.Drive.maxVelocity,
Dashboard.Drive.maxAcceleration,
Dashboard.Drive.maxDeceleration,
tuneDriveAtEndPoint? tuneDriveStartPoint: tuneDriveEndPoint);
tuneDriveAtEndPoint = !tuneDriveAtEndPoint; tuneDriveAtEndPoint = !tuneDriveAtEndPoint;
} }
passToTeleOp = false; passToTeleOp = false;
@ -863,7 +874,8 @@ public class FtcTest extends FtcTeleOp
private boolean allowTeleOp() private boolean allowTeleOp()
{ {
return teleOpControlEnabled && return teleOpControlEnabled &&
(testChoices.test == Test.SUBSYSTEMS_TEST || testChoices.test == Test.DRIVE_SPEED_TEST); (testChoices.test == Test.SUBSYSTEMS_TEST || testChoices.test == Test.VISION_TEST ||
testChoices.test == Test.DRIVE_SPEED_TEST);
} //allowTeleOp } //allowTeleOp
} //class FtcTest } //class FtcTest