Skip to content

Commit

Permalink
camera offsets and more calibration
Browse files Browse the repository at this point in the history
  • Loading branch information
@gerth2
gerth2 committed Oct 18, 2024
1 parent 570fbba commit 36d362a
Show file tree
Hide file tree
Showing 4 changed files with 36 additions and 50 deletions.
8 changes: 4 additions & 4 deletions drivetrain/drivetrainControl.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
25 changes: 12 additions & 13 deletions drivetrain/drivetrainPhysical.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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(
Expand Down
10 changes: 6 additions & 4 deletions drivetrain/swerveModuleControl.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
43 changes: 14 additions & 29 deletions drivetrain/swerveModuleGainSet.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
"""
Expand Down

0 comments on commit 36d362a

Please sign in to comment.