Merge pull request #9 from 1757WestwoodRobotics/rev-modules

REV modules

commands/absoluterelativedrive.py

@@ -48,8 +48,8 @@ def rotation(self) -> float:
 
     def execute(self) -> None:
         self.drive.arcadeDriveWithFactors(
-            self.forward(),
-            self.sideways(),
+            -self.forward(),
+            -self.sideways(),
             self.rotation(),
             DriveSubsystem.CoordinateMode.FieldRelative,
         )

commands/fieldrelativedrive.py

@@ -24,8 +24,8 @@ def __init__(
 
     def execute(self) -> None:
         self.drive.arcadeDriveWithFactors(
-            self.forward(),
-            self.sideways(),
+            -self.forward(),
+            -self.sideways(),
             self.rotation(),
             DriveSubsystem.CoordinateMode.FieldRelative,
         )

commands/hornhonk.py

@@ -1,21 +1,19 @@
-import typing
 from subsystems.hornsubsystem import HornSubsystem
 from commands2 import CommandBase
 
 
 class HornHonk(CommandBase):
-    def __init__(self, horn: HornSubsystem, amount: typing.Callable[[], float]) -> None:
+    def __init__(self, horn: HornSubsystem) -> None:
         CommandBase.__init__(self)
         self.horn = horn
-        self.hornStrength = amount
 
         self.hornOutput = lambda strength: self.horn.horn.set(strength)
 
         self.setName(__class__.__name__)
         self.addRequirements([self.horn])
 
     def execute(self) -> None:
-        self.hornOutput(self.hornStrength())
+        self.hornOutput(1.0)
 
     def end(self, interrupted: bool) -> None:
         self.hornOutput(0.0)

commands/returndrive.py

@@ -9,14 +9,12 @@ class ReturnDrive(CommandBase):
     def __init__(
         self,
         drive: DriveSubsystem,
-        scaler: typing.Callable[[], float],
         rotation: typing.Callable[[], float],
     ) -> None:
         CommandBase.__init__(self)
         self.setName(__class__.__name__)
 
         self.drive = drive
-        self.scaler = scaler
         self.rotation = rotation
 
         self.addRequirements([self.drive])
@@ -52,8 +50,8 @@ def execute(self) -> None:
         deadband = 0.1
 
         self.drive.arcadeDriveWithFactors(
-            self.Deadband(self.getDirection()[0], deadband) * self.scaler(),
-            self.Deadband(self.getDirection()[1], deadband) * self.scaler(),
+            self.Deadband(self.getDirection()[0], deadband),
+            self.Deadband(self.getDirection()[1], deadband),
             self.rotation() * -1,
             DriveSubsystem.CoordinateMode.FieldRelative,
         )

constants.py

@@ -15,7 +15,6 @@
 import math
 from wpimath.geometry import Translation2d
 from wpimath.system.plant import DCMotor
-from foxglove import FoxglovePublisher
 
 # Basic units
 kCentimetersPerInch = 2.54
@@ -24,6 +23,9 @@
 kCentimetersPerMeter = 100
 """centimeters / meter"""
 
+kSecondsPerMinute = 60
+"""seconds / minutes"""
+
 kMetersPerInch = kCentimetersPerInch / kCentimetersPerMeter
 """meters / inch"""
 
@@ -211,6 +213,15 @@
 )
 """pulses / radian"""
 
+# REV Modules
+kSwerveMotorDegreesPerEncoderRev = kDegeersPerRevolution / kSteerGearingRatio
+"""end degrees / motor revolution"""
+kSwerveMetersPerDriveEncoderRevolution = (kWheelDiameter * math.pi) / kDriveGearingRatio
+"""drive meters / motor revolution"""
+# in one minutes, 1rpm encoder moves at kSwerveMotorDegreesPerEncoderRev, so in 1 second it travels at 1/60th
+kSwerveDriveEncoderRPMperMPS = kSwerveMetersPerDriveEncoderRevolution / 60
+"""motor rpm / wheel mps"""
+
 # CTRE
 k100MillisecondsPerSecond = 10 / 1  # there are 10 groups of 100 milliseconds per second
 """100 milliseconds / second
@@ -221,14 +232,14 @@
 """milliseconds"""
 
 kDrivePIDSlot = 0
-kDrivePGain = 0.12
+kDrivePGain = 0.4
 kDriveIGain = 0.0
-kDriveDGain = 0.0
+kDriveDGain = 0.06
 
 kSteerPIDSlot = 0
-kSteerPGain = 0.6
+kSteerPGain = 0.03
 kSteerIGain = 0.0
-kSteerDGain = 12.0
+kSteerDGain = 8e-5
 
 kFrontLeftDriveInverted = False
 kFrontRightDriveInverted = True
@@ -253,16 +264,16 @@
   7. Click "Self-Test Snapshot"
   8. Record value from line: "Absolute Position (unsigned):"
 """
-kFrontLeftAbsoluteEncoderOffset = 314.912
+kFrontLeftAbsoluteEncoderOffset = 94.482
 """degrees"""
 
-kFrontRightAbsoluteEncoderOffset = 5.713
+kFrontRightAbsoluteEncoderOffset = 83.408
 """degrees"""
 
-kBackLeftAbsoluteEncoderOffset = 19.424
+kBackLeftAbsoluteEncoderOffset = 227.988
 """degrees"""
 
-kBackRightAbsoluteEncoderOffset = 142.998
+kBackRightAbsoluteEncoderOffset = 140.449
 """degrees"""
 
 kRobotPoseArrayKeys = "RobotPoseArray"
@@ -286,7 +297,7 @@
 
 # drive control related values
 
-kRotationPGain = 0.8
+kRotationPGain = 0.5
 kRotationIGain = 0
 kRotationDGain = 0
 
@@ -339,14 +350,14 @@
 kPressureInMin = 0
 kPressureInMax = 200
 
-#power stuff
+# power stuff
 kRobotPowerChannelsKey = "powerDistribution"
 kRobotVoltageChannelKey = "powerVoltage"
 
-foxglove_sub_topics = {
-    "RobotPose": (kRobotPoseArrayKeys, FoxglovePublisher.FoxgloveType.Pose2d),
-    "CannonState": (kCannonStateKey, FoxglovePublisher.FoxgloveType.Number),
-    "PowerChannels": (kRobotPowerChannelsKey, FoxglovePublisher.FoxgloveType.NumberArray),
-    "Voltage": (kRobotVoltageChannelKey, FoxglovePublisher.FoxgloveType.Number),
-    "Pressure": (kPressureKey, FoxglovePublisher.FoxgloveType.Number)
-}
+# foxglove_sub_topics = {
+#     "RobotPose": (kRobotPoseArrayKeys, FoxglovePublisher.FoxgloveType.Pose2d),
+#     "CannonState": (kCannonStateKey, FoxglovePublisher.FoxgloveType.Number),
+#     "PowerChannels": (kRobotPowerChannelsKey, FoxglovePublisher.FoxgloveType.NumberArray),
+#     "Voltage": (kRobotVoltageChannelKey, FoxglovePublisher.FoxgloveType.Number),
+#     "Pressure": (kPressureKey, FoxglovePublisher.FoxgloveType.Number)
+# }