amarcolini
commented
2 years ago Joos does not currently have support for differential swerve out of the box, so you would have to write the odometry and kinematics yourself. It might end up looking something like this:
/**
* @param trackWidth the distance between the two modules
*/
class DifferentialSwerveDrive(
private val leftMotorA: Motor, private val leftMotorB: Motor,
private val rightMotorA: Motor, private val rightMotorB: Motor,
private val trackWidth: Double,
private val imu: Imu? = null
) : Drive(), Component {
/*
Note that a motor A corresponds to the motor that spins the same direction as the wheel,
and motor B spins the opposite direction.
*/
/**
* The gear ratio from one of the motors to the wheel shaft.
*/
val gearRatio: Double = 1.0
/**
* The ticks per rev of each motor.
*/
val ticksPerRev: Double = 100.0
/**
* The radius of each wheel.
*/
val wheelRadius: Double = 2.0
/**
* This is for controlling the orientation of each module. You'll have to tune this.
*/
val modulePID = PIDCoefficients(4.0, 0.0, 0.1)
override var localizer: Localizer = DifferentialSwerveLocalizer(
::getWheelPositions, ::getWheelVelocities, ::getModuleOrientations,
trackWidth, this, imu != null
)
override val rawExternalHeading: Double = imu.heading ?: 0.0
private val leftModuleController = PIDFController(modulePID)
private val rightModuleController = PIDFController(modulePID)
private val motors = listOf(leftMotorA, leftMotorB, rightMotorA, rightMotorB)
init {
leftModuleController.setInputBounds(-PI, PI)
rightModuleController.setInputBounds(-PI, PI)
motors.forEach {
it.distancePerRev = 2 * PI * wheelRadius * gearRatio
//TODO: tune feedforward for motors
it.feedforwardCoefficients == FeedforwardCoefficients()
}
}
private var leftVel = 0.0
private var leftAccel = 0.0
private var rightVel = 0.0
private var rightAccel = 0.0
private fun getWheelPositions(): Pair<Double, Double> =
(leftMotorA.distance - leftMotorB.distance) * 0.5 to (rightMotorA.distance - rightMotorB.distance) * 0.5
private fun getWheelVelocities(): Pair<Double, Double> =
(leftMotorA.distanceVelocity - leftMotorB.distanceVelocity) * 0.5 to (rightMotorA.distanceVelocity - rightMotorB.distanceVelocity) * 0.5
private fun getModuleOrientations(): Pair<Double, Double> =
(leftMotorA.currentPosition + leftMotorB.currentPosition) / ticksPerRev * PI to (rightMotorA.currentPosition + rightMotorB) / ticksPerRev * PI
override fun setDrivePower(drivePower: Pose2d) {
val leftVector = Vector2d(drivePower.x, drivePower.y + drivePower.heading)
val rightVector = Vector2d(drivePower.x, drivePower.y - drivePower.heading)
leftModuleController.setTarget(leftVector.angle())
rightModuleController.setTarget(rightVector.angle())
leftVel = leftVector.norm() * leftMotorA.maxRPM
leftAccel = 0.0
rightVel = rightVector.norm() * rightMotorA.maxRPM
rightAccel = 0.0
}
override fun update() {
val moduleOrientations = getModuleOrientations()
val leftControl = leftModuleController.update(
moduleOrientations.first
)
leftMotorA.setSpeed(leftVel + leftControl, leftAccel)
leftMotorB.setSpeed(-leftVel + leftControl, -leftAccel)
val rightControl = rightModuleController.update(
moduleOrientations.second
)
rightMotorA.setSpeed(rightVel + rightControl, rightAccel)
rightMotorB.setSpeed(-rightVel + rightControl, -rightAccel)
motors.forEach { it.update() }
}
override fun setDriveSignal(driveSignal: DriveSignal) {
val leftVelVector = Vector2d(driveSignal.vel.x, driveSignal.vel.y + driveSignal.vel.heading)
val rightVelVector = Vector2d(driveSignal.vel.x, driveSignal.vel.y - driveSignal.vel.heading)
val leftAccelVector = Vector2d(driveSignal.accel.x, driveSignal.accel.y + driveSignal.accel.heading)
val rightAccelVector = Vector2d(driveSignal.accel.x, driveSignal.accel.y - driveSignal.accel.heading)
leftModuleController.setTarget(leftVelVector.angle())
rightModuleController.setTarget(rightVelVector.angle())
val divisor = (2 * PI * wheelRadius * gearRatio)
leftVel = leftVelVector.norm() / divisor
leftAccel = leftAccelVector.norm() / divisor
rightVel = rightVelVector.norm() / divisor
rightAccel = rightAccelVector.norm() / divisor
}
override fun getExternalHeadingVelocity(): Double? = imu?.headingVelocity
}
class DifferentialSwerveLocalizer(
private val wheelPositions: () -> Pair<Double, Double>,
private val wheelVelocities: () -> Pair<Double, Double>? = { null },
private val moduleOrientations: () -> Pair<Double, Double>,
private val trackWidth: Double,
private val drive: Drive,
private val useExternalHeading: Boolean = true
) : Localizer {
private var _poseEstimate: Pose2d = Pose2d()
override var poseEstimate: Pose2d
get() = _poseEstimate
set(value) {
lastWheelPositions = null
lastExtHeading = null
if (useExternalHeading) drive.externalHeading = value.heading
_poseEstimate = value
}
private var lastWheelPositions: Pair<Double, Double>? = null
private var lastExtHeading: Double? = null
override var poseVelocity: Pose2d? = null
private set
override fun update() {
val wheelPositions = wheelPositions()
val moduleOrientations = moduleOrientations()
val externalHeading = if (useExternalHeading) drive.externalHeading else null
lastWheelPositions?.let { lastWheelPositions ->
val leftDelta = wheelPositions.first - lastWheelPositions.first
val rightDelta = wheelPositions.second - lastWheelPositions.second
val robotPoseDelta = Pose2d(
(leftDelta * cos(moduleOrientations.first) + rightDelta * cos(moduleOrientations.second)) * 0.5,
(leftDelta * sin(moduleOrientations.first) + rightDelta * sin(moduleOrientations.second)) * 0.5,
lastExtHeading?.let { externalHeading?.minus(it) }
?: ((rightDelta * sin(moduleOrientations.second) - leftDelta * sin(moduleOrientations.first)) / trackWidth)
)
_poseEstimate = Kinematics.relativeOdometryUpdate(
_poseEstimate, robotPoseDelta
)
}
val wheelVelocities = wheelVelocities()
val extHeadingVel = if (useExternalHeading) drive.getExternalHeadingVelocity() else null
poseVelocity =
if (wheelVelocities != null)
Pose2d(
(wheelVelocities.first * cos(moduleOrientations.first) + wheelVelocities.second * cos(
moduleOrientations.second
)) * 0.5,
(wheelVelocities.first * sin(moduleOrientations.first) + wheelVelocities.second * sin(
moduleOrientations.second
)) * 0.5,
extHeadingVel
?: ((wheelVelocities.second * sin(moduleOrientations.second) - wheelVelocities.first * sin(
moduleOrientations.first
)) / trackWidth)
)
else null
lastWheelPositions = wheelPositions
lastExtHeading = externalHeading
}
}Note that this may or may not work, but should give you an idea of what to do.
omagarwal25
How would I go about implementing a custom drivetrain? We have a differential 2 moduled swerve and I was wondering how to create it so that I can use all the odometry and kinematics.