diff --git a/rio/commands/manualRot.py b/rio/commands/manualRot.py index 75886e7..5d61b46 100644 --- a/rio/commands/manualRot.py +++ b/rio/commands/manualRot.py @@ -8,7 +8,7 @@ def __init__(self, speed, _shooter: Shooter): super().__init__() # local subsystem instance - self.subsystem = Shooter + self.subsystem = _shooter # requested speed self.speed = speed diff --git a/rio/robot.py b/rio/robot.py index d5e457b..a0d4170 100644 --- a/rio/robot.py +++ b/rio/robot.py @@ -12,7 +12,7 @@ from robotcontainer import RobotContainer -class MyRobot(commands2.TimedCommandRobot): +class HolyToaster(commands2.TimedCommandRobot): def robotInit(self): # Instantiate our RobotContainer. This will perform all our button bindings, and put our # autonomous chooser on the dashboard. @@ -45,4 +45,4 @@ def testInit(self) -> None: if __name__ == "__main__": - wpilib.run(MyRobot) + wpilib.run(HolyToaster) diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 3e78ab3..1aa654a 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -18,7 +18,7 @@ "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "283,140" + "Size": "169,184" }, "###Joysticks": { "Collapsed": "0", @@ -27,13 +27,13 @@ }, "###NetworkTables": { "Collapsed": "0", - "Pos": "250,277", - "Size": "750,185" + "Pos": "250,163", + "Size": "750,299" }, "###NetworkTables Info": { "Collapsed": "0", - "Pos": "250,130", - "Size": "750,145" + "Pos": "250,22", + "Size": "750,138" }, "###Other Devices": { "Collapsed": "0", diff --git a/rio/simgui.json b/rio/simgui.json index e5ab25b..97e5060 100644 --- a/rio/simgui.json +++ b/rio/simgui.json @@ -20,13 +20,17 @@ }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/SmartDashboard/FieldCentric": "String Chooser" } }, "NetworkTables": { "transitory": { "SmartDashboard": { "Swerve": { + "FL": { + "open": true + }, "open": true }, "open": true diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index 57041a1..c32604a 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/rio/subsystems/drivesubsystem.py @@ -101,25 +101,51 @@ def periodic(self) -> None: ), ) - # desired + # Desired Positions self.sd.putNumber( - "Swerve/FL desired", self.frontLeft.desiredState.angle.degrees() + "Swerve/FL/desired", + self.frontLeft.desiredState.angle.degrees(), ) self.sd.putNumber( - "Swerve/FR desired", self.frontRight.desiredState.angle.degrees() + "Swerve/FR/desired", + self.frontRight.desiredState.angle.degrees(), ) self.sd.putNumber( - "Swerve/RL desired", self.rearLeft.desiredState.angle.degrees() + "Swerve/RL/desired", + self.rearLeft.desiredState.angle.degrees(), ) self.sd.putNumber( - "Swerve/RR desired", self.rearRight.desiredState.angle.degrees() + "Swerve/RR/desired", + self.rearRight.desiredState.angle.degrees(), ) - # actual - self.sd.putNumber("Swerve/FL", self.frontLeft.getState().angle.degrees()) - self.sd.putNumber("Swerve/FR", self.frontRight.getState().angle.degrees()) - self.sd.putNumber("Swerve/RL", self.rearLeft.getState().angle.degrees()) - self.sd.putNumber("Swerve/RR", self.rearRight.getState().angle.degrees()) + # Actual Positions + self.sd.putNumber( + "Swerve/FL/actual", + self.frontLeft.getState().angle.degrees(), + ) + self.sd.putNumber( + "Swerve/FR/actual", + self.frontRight.getState().angle.degrees(), + ) + self.sd.putNumber( + "Swerve/RL/actual", + self.rearLeft.getState().angle.degrees(), + ) + self.sd.putNumber( + "Swerve/RR/actual", + self.rearRight.getState().angle.degrees(), + ) + + # Thermals + self.sd.putNumber("Thermals/Swerve/FL/drive", self.frontLeft.getDriveTemp()) + self.sd.putNumber("Thermals/Swerve/FR/drive", self.frontRight.getDriveTemp()) + self.sd.putNumber("Thermals/Swerve/RL/drive", self.rearLeft.getDriveTemp()) + self.sd.putNumber("Thermals/Swerve/RR/drive", self.rearRight.getDriveTemp()) + self.sd.putNumber("Thermals/Swerve/FL/turn", self.frontLeft.getTurnTemp()) + self.sd.putNumber("Thermals/Swerve/FR/turn", self.frontRight.getTurnTemp()) + self.sd.putNumber("Thermals/Swerve/RL/turn", self.rearLeft.getTurnTemp()) + self.sd.putNumber("Thermals/Swerve/RR/turn", self.rearRight.getTurnTemp()) def getPose(self) -> Pose2d: """Returns the currently-estimated pose of the robot. diff --git a/rio/subsystems/mikeswervemodule.py b/rio/subsystems/mikeswervemodule.py index 15dd474..48d4716 100644 --- a/rio/subsystems/mikeswervemodule.py +++ b/rio/subsystems/mikeswervemodule.py @@ -166,9 +166,16 @@ def setDesiredState(self, desiredState: SwerveModuleState) -> None: ) self.desiredState = desiredState - + def resetEncoders(self) -> None: """ Zeroes all the SwerveModule encoders. """ self.drivingEncoder.setPosition(0) + + def getDriveTemp(self) -> float: + return self.drivingSparkMax.getMotorTemperature() + + def getTurnTemp(self) -> float: + return self.turningSparkMax.getMotorTemperature() + \ No newline at end of file