Angle check

[JosephTLockwood]

Add an angle check to prevent the following from being printed continuously (may need to be added on some other getAngle() as well.

Error at java.base/java.lang.Thread.getStackTrace(Thread.java:1619): x and y components of Rotation2d are zero

        at java.base/java.lang.Thread.getStackTrace(Thread.java:1619)
        at edu.wpi.first.math.geometry.Rotation2d.<init>(Rotation2d.java:122)
        at edu.wpi.first.math.geometry.Translation2d.getAngle(Translation2d.java:174)
        at com.pathplanner.lib.util.swerve.SwerveSetpointGenerator.generateSetpoint(SwerveSetpointGenerator.java:345)
       ....
        at com.ctre.phoenix6.swerve.SwerveDrivetrain.setControl(SwerveDrivetrain.java:413)
        at frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain.lambda$14(CommandSwerveDrivetrain.java:266)
        at edu.wpi.first.wpilibj2.command.FunctionalCommand.execute(FunctionalCommand.java:57)
        at edu.wpi.first.wpilibj2.command.CommandScheduler.run(CommandScheduler.java:288)
        at frc.robot.Robot.robotPeriodic(Robot.java:27)
        at edu.wpi.first.wpilibj.IterativeRobotBase.loopFunc(IterativeRobotBase.java:400)
        at edu.wpi.first.wpilibj.TimedRobot.startCompetition(TimedRobot.java:131)
tRobot$0(RobotBase.java:418)
        at java.base/java.lang.Thread.run(Thread.java:840)

[mjansen4857]

These same checks need to be added to C++ and python as well, those don't have the setpoint generator so its only needed in the trajectory. Also, you should cache the value of wheelForces[m].getNorm() so that it doesn't need to be calculated twice.

[JosephTLockwood]

@mjansen4857 Should be good to go now. I wasn't sure how to format Python, though.