diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py index 204da7dab2..25be5d19e9 100644 --- a/photonlib-python-examples/poseest/drivetrain.py +++ b/photonlib-python-examples/poseest/drivetrain.py @@ -156,11 +156,13 @@ def log(self): self.backLeft.log() self.backRight.log() - self.debugField.getObject("EstimatedRobot").setPose(self.odometry.getPose()) - self.debugField.getObject("EstimatedRobotModules").setPoses(self.getModulePoses()) + self.debugField.getRobotObject().setPose(self.odometry.getPose()) + self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses()) def simulationPeriodic(self): self.frontLeft.simulationPeriodic() self.frontRight.simulationPeriodic() self.backLeft.simulationPeriodic() - self.backRight.simulationPeriodic() \ No newline at end of file + self.backRight.simulationPeriodic() + self.simGyro.setRate(self.getChassisSpeeds().omega_dps) + self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02) \ No newline at end of file diff --git a/photonlib-python-examples/poseest/simgui.json b/photonlib-python-examples/poseest/simgui.json index f7cfb3805d..2a9feb9f77 100644 --- a/photonlib-python-examples/poseest/simgui.json +++ b/photonlib-python-examples/poseest/simgui.json @@ -9,6 +9,13 @@ "window": { "visible": true } + }, + "Other Devices": { + "AnalogGyro[0]": { + "header": { + "open": true + } + } } }, "NTProvider": { @@ -18,10 +25,10 @@ }, "windows": { "/SmartDashboard/Drivetrain Debug": { - "EstimatedRobotModules": { - "image": "D:\\Projects\\photonvision\\photonlib-python-examples\\poseest\\swerve_module.png", - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "SwerveModules": { + "image": "swerve_module.png", + "length": 0.5, + "width": 0.5 }, "bottom": 1476, "height": 8.210550308227539, @@ -39,16 +46,13 @@ "transitory": { "SmartDashboard": { "Drivetrain Debug": { - "double[]##v_/SmartDashboard/Drivetrain Debug/EstimatedRobot": { - "open": true - }, - "double[]##v_/SmartDashboard/Drivetrain Debug/EstimatedRobotModules": { - "open": true - }, "double[]##v_/SmartDashboard/Drivetrain Debug/Robot": { "open": true } }, + "Module 1": { + "open": true + }, "open": true } } diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py index e3056993dc..c8c05e4aba 100644 --- a/photonlib-python-examples/poseest/swervemodule.py +++ b/photonlib-python-examples/poseest/swervemodule.py @@ -52,22 +52,14 @@ def __init__( ) # Gains are for example purposes only - must be determined for your own robot! - self.drivePIDController = wpimath.controller.PIDController(1, 0, 0) + self.drivePIDController = wpimath.controller.PIDController(10, 0, 0) # Gains are for example purposes only - must be determined for your own robot! - self.turningPIDController = wpimath.controller.ProfiledPIDController( - 3, - 0, - 0, - wpimath.trajectory.TrapezoidProfile.Constraints( - kModuleMaxAngularVelocity, - kModuleMaxAngularAcceleration, - ), - ) + self.turningPIDController = wpimath.controller.PIDController(30,0,0) # Gains are for example purposes only - must be determined for your own robot! self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) - self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.5) + self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7) # Set the distance per pulse for the drive encoder. We can simply use the # distance traveled for one rotation of the wheel divided by the encoder @@ -150,7 +142,7 @@ def setDesiredState( ) turnFeedforward = self.turnFeedforward.calculate( - self.turningPIDController.getSetpoint().velocity + self.turningPIDController.getSetpoint() ) self.driveMotor.setVoltage(driveOutput + driveFeedforward) @@ -166,7 +158,7 @@ def log(self) -> None: wpilib.SmartDashboard.putNumber( table + "Steer Degrees", math.degrees(wpimath.angleModulus(state.angle.radians()))) wpilib.SmartDashboard.putNumber( - table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint().position)) + table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint())) wpilib.SmartDashboard.putNumber( table + "Drive Velocity Feet", state.speed_fps) wpilib.SmartDashboard.putNumber(