From 36d362ae9543a2c59d01b268ca6e8696863701cf Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Thu, 17 Oct 2024 22:11:08 -0500 Subject: [PATCH] camera offsets and more calibration --- drivetrain/drivetrainControl.py | 8 +++--- drivetrain/drivetrainPhysical.py | 25 +++++++++--------- drivetrain/swerveModuleControl.py | 10 ++++--- drivetrain/swerveModuleGainSet.py | 43 ++++++++++--------------------- 4 files changed, 36 insertions(+), 50 deletions(-) diff --git a/drivetrain/drivetrainControl.py b/drivetrain/drivetrainControl.py index c19ec90..5ed2ce6 100644 --- a/drivetrain/drivetrainControl.py +++ b/drivetrain/drivetrainControl.py @@ -39,19 +39,19 @@ def __init__(self): self.modules = [] self.modules.append( SwerveModuleControl("FL", DT_FL_WHEEL_CANID, DT_FL_AZMTH_CANID, DT_FL_AZMTH_ENC_PORT, - FL_ENCODER_MOUNT_OFFSET_RAD, False, True) + FL_ENCODER_MOUNT_OFFSET_RAD, True, True) ) self.modules.append( SwerveModuleControl("FR", DT_FR_WHEEL_CANID, DT_FR_AZMTH_CANID, DT_FR_AZMTH_ENC_PORT, - FR_ENCODER_MOUNT_OFFSET_RAD, True, False) + FR_ENCODER_MOUNT_OFFSET_RAD, True, True) ) self.modules.append( SwerveModuleControl("BL", DT_BL_WHEEL_CANID, DT_BL_AZMTH_CANID, DT_BL_AZMTH_ENC_PORT, - BL_ENCODER_MOUNT_OFFSET_RAD, True, True) + BL_ENCODER_MOUNT_OFFSET_RAD, False, True) ) self.modules.append( SwerveModuleControl("BR", DT_BR_WHEEL_CANID, DT_BR_AZMTH_CANID, DT_BR_AZMTH_ENC_PORT, - BR_ENCODER_MOUNT_OFFSET_RAD, True, True) + BR_ENCODER_MOUNT_OFFSET_RAD, False, True) ) self.desChSpd = ChassisSpeeds() diff --git a/drivetrain/drivetrainPhysical.py b/drivetrain/drivetrainPhysical.py index b592610..d3a4c4e 100644 --- a/drivetrain/drivetrainPhysical.py +++ b/drivetrain/drivetrainPhysical.py @@ -105,16 +105,15 @@ def dtMotorRotToLinear(rot): if RobotIdentification().getRobotType() == RobotTypes.Main: - FL_ENCODER_MOUNT_OFFSET_RAD = 2.489 - FR_ENCODER_MOUNT_OFFSET_RAD = 2.340 - BL_ENCODER_MOUNT_OFFSET_RAD = -2.208 - BR_ENCODER_MOUNT_OFFSET_RAD = -1.168 + FR_ENCODER_MOUNT_OFFSET_RAD = 0.8412 + FL_ENCODER_MOUNT_OFFSET_RAD = 0.2412 + BR_ENCODER_MOUNT_OFFSET_RAD = 1.259 + BL_ENCODER_MOUNT_OFFSET_RAD = 1.777 else: - FL_ENCODER_MOUNT_OFFSET_RAD = 2.489 - FR_ENCODER_MOUNT_OFFSET_RAD = 2.340 - BL_ENCODER_MOUNT_OFFSET_RAD = -2.208 - BR_ENCODER_MOUNT_OFFSET_RAD = -1.168 - + FR_ENCODER_MOUNT_OFFSET_RAD = 0.8412 + FL_ENCODER_MOUNT_OFFSET_RAD = 0.2412 + BR_ENCODER_MOUNT_OFFSET_RAD = 1.259 + BL_ENCODER_MOUNT_OFFSET_RAD = 1.777 # Module Indices (for ease of array manipulation) FL = 0 @@ -127,16 +126,16 @@ def dtMotorRotToLinear(rot): # which is in the center of the chassis on the ground ROBOT_TO_LEFT_CAM = Transform3d( Translation3d( - inchesToMeters(11.0), inchesToMeters(5.0), inchesToMeters(3.0) # X # Y # Z + inchesToMeters(3.7), inchesToMeters(13.8), inchesToMeters(7.4) # X # Y # Z ), - Rotation3d(0.0, 0.0, 0.0), # Roll # Pitch # Yaw + Rotation3d.fromDegrees(0, -30, 90.0), # Roll # Pitch # Yaw ) ROBOT_TO_RIGHT_CAM = Transform3d( Translation3d( - inchesToMeters(11.0), inchesToMeters(-5.0), inchesToMeters(3.0) # X # Y # Z + inchesToMeters(3.7), inchesToMeters(-13.8), inchesToMeters(7.4) # X # Y # Z ), - Rotation3d(0.0, 0.0, 0.0), # Roll # Pitch # Yaw + Rotation3d.fromDegrees(0, -30, -90.0), # Roll # Pitch # Yaw ) ROBOT_TO_FRONT_CAM = Transform3d( diff --git a/drivetrain/swerveModuleControl.py b/drivetrain/swerveModuleControl.py index c3ac285..529d6f5 100644 --- a/drivetrain/swerveModuleControl.py +++ b/drivetrain/swerveModuleControl.py @@ -46,7 +46,7 @@ def __init__( azmthEncoderPortIdx (int): RIO Port for the azimuth absolute encoder for this module azmthOffset (float): Mounting offset of the azimuth encoder in Radians. invertWheel (bool): Inverts the drive direction of the wheel - needed since left/right sides are mirrored - invertWheel (bool): Inverts the steering direction of the wheel - needed if motor is mounted upside + invertAzmth (bool): Inverts the steering direction of the wheel - needed if motor is mounted upside """ self.wheelMotor = WrapperedSparkMax( wheelMotorCanID, moduleName + "_wheel", False @@ -175,9 +175,11 @@ def update(self): self.actualPosition.angle = self.actualState.angle # Optimize our incoming swerve command to minimize motion - self.optimizedDesiredState = SwerveModuleState.optimize( - self.desiredState, self.actualState.angle - ) + #self.optimizedDesiredState = SwerveModuleState.optimize( + # self.desiredState, self.actualState.angle + #) + # Skip optimization for now + self.optimizedDesiredState = self.desiredState # Use a PID controller to calculate the voltage for the azimuth motor self.azmthCtrl.setSetpoint(self.optimizedDesiredState.angle.degrees()) # type: ignore diff --git a/drivetrain/swerveModuleGainSet.py b/drivetrain/swerveModuleGainSet.py index d310913..931138c 100644 --- a/drivetrain/swerveModuleGainSet.py +++ b/drivetrain/swerveModuleGainSet.py @@ -12,35 +12,20 @@ class SwerveModuleGainSet: """ def __init__(self): - if RobotIdentification().getRobotType() == RobotTypes.Practice: - self.wheelP = Calibration("Drivetrain Module Wheel kP", 0.00005) - self.wheelI = Calibration("Drivetrain Module Wheel kI", 0.0) - self.wheelD = Calibration("Drivetrain Module Wheel kD", 0.0) - self.wheelA = Calibration( - "Drivetrain Module Wheel kA", 0.000, "volts/radPerSecPerSec" - ) - self.wheelV = Calibration( - "Drivetrain Module Wheel kV", 12.0 / RPM2RadPerSec(4700), "volts/radPerSec" - ) - self.wheelS = Calibration("Drivetrain Module Wheel kS", 0.12, "volts") - self.azmthP = Calibration("Drivetrain Module Azmth kP", 0.115) - self.azmthI = Calibration("Drivetrain Module Azmth kI", 0.0) - self.azmthD = Calibration("Drivetrain Module Azmth kD", 0.0001) - else: - self.wheelP = Calibration("Drivetrain Module Wheel kP", 0.00005) - self.wheelI = Calibration("Drivetrain Module Wheel kI", 0.0) - self.wheelD = Calibration("Drivetrain Module Wheel kD", 0.0) - self.wheelA = Calibration( - "Drivetrain Module Wheel kA", 0.000, "volts/radPerSecPerSec" - ) - self.wheelV = Calibration( - "Drivetrain Module Wheel kV", 12.0 / RPM2RadPerSec(4700), "volts/radPerSec" - ) - self.wheelS = Calibration("Drivetrain Module Wheel kS", 0.12, "volts") - self.azmthP = Calibration("Drivetrain Module Azmth kP", 0.115) - self.azmthI = Calibration("Drivetrain Module Azmth kI", 0.0) - self.azmthD = Calibration("Drivetrain Module Azmth kD", 0.0001) - #If we're not on the serial number of the practice bot, we want to assume we're on the main + + self.wheelP = Calibration("Drivetrain Module Wheel kP", 0.00005) + self.wheelI = Calibration("Drivetrain Module Wheel kI", 0.0) + self.wheelD = Calibration("Drivetrain Module Wheel kD", 0.0) + self.wheelA = Calibration( + "Drivetrain Module Wheel kA", 0.000, "volts/radPerSecPerSec" + ) + self.wheelV = Calibration( + "Drivetrain Module Wheel kV", 12.0 / RPM2RadPerSec(4700), "volts/radPerSec" + ) + self.wheelS = Calibration("Drivetrain Module Wheel kS", 0.12, "volts") + self.azmthP = Calibration("Drivetrain Module Azmth kP", 0.02) + self.azmthI = Calibration("Drivetrain Module Azmth kI", 0.0) + self.azmthD = Calibration("Drivetrain Module Azmth kD", 0.0000) def hasChanged(self): """