Whenever I end an OpMode mannually but a command is still running I get a ConcurrentModificationException
Closed this issue · 36 comments
I've tried this using wait commands and with just a regular command with isFinished always set to false. Here is an example of a code that would cause this error
val driver =
WaitCommand(20.0)
.onExecute {
val leftStickX = gamepad1.left_stick_x.toDouble()
val leftStickY = gamepad1.left_stick_y.toDouble()
val rightStickX = gamepad1.right_stick_x.toDouble()
drive.setDrivePower(Pose2d(leftStickX, leftStickY, rightStickX) * 0.45)
}
//.requires(drive)
//.onEnd { drive.setDrivePower(Pose2d(0.0, 0.0, 0.0)) }
// .runUntil(false)
schedule(driver)
Are you using a RobotOpMode?
Yes I am, I have a light wrapper but it doesn't override any functions.
And you're stopping the OpMode while it's still running?
Yeah from the phone, if the command ends before the disable it's fine
I've found the issue. CommandScheduler.reset() is not currently thread-safe, but it should be fixed in the next release. Once that comes out you can close this issue if it's fixed.
Is it an easy fix? I could try to do it if you can isolate the issue. We have a competition coming up in less than a month
0.4.0 is out - give it a whirl!
@amarcolini Thank you so much, I have been making tweaks to the swerve code as I have been testing so I will have a few PRs in the coming days in case things don't quite work, such as optimization.
@amarcolini Where do I set up my gear ratios for my modules? Do I do it motor by motor and just change distancePerRev? Look at the code TPR is for the angular rotation of the module now?
@omagarwal25 you would have to do it motor by motor, but you can do it via the constructor:
Motor(hMap, "left_top", 312.0, 537.7, 2.5, 2.0)Where 2.5 and 2.0 are the wheel radius and gear ratio, respectively.
Okay so really small change. In order for a swerve drive to be optimized, it should not have to spin 180 degrees to go in the opposite direction. This small tweak fixes all of that
First, you need to change the PID so that it it looks like this 
so that it doesn't care of it's 90 degrees or 270 degrees as those are the same
Then when determining the velocity instead of just doing a straight map, you need to check if the target is within 90 degrees of the current and of not you should reverse the acceleration and the velocity of the wheel so that it goes backwards, something like this.
I hope this makes sense, I can implement all the necessary code if you tell me where to put it. I have put it all in update but I guess it would not fit the current kinematics structure.
I'm not quite sure how the first one takes into the account the position of the modules. You have to take into account which direction the module is facing to decide the direction. Because it could be at -PI and you want to go forward so you actually have to go backwards with the wheels.
Well if the orientation angle is clamped to [-PI/2, PI/2], all the module vectors that fall in that range are in quadrants I and IV, which both have positive x values. If a module vector falls outside of that range, its x value is negative. Thus, multiplying the wheel velocity by the sign of the x value of the module vector will appropriately reverse its direction.
Wait those vectors are the current not the target? Okay that makes sense.
@amarcolini you cannot initialize target speeds as an empty list because it will kill the code as soon as diff.update() is called.
You're right
@amarcolini Additionally, for robotToWheelVelocities, you can't just take the quadrant and reverse. The module could be at -.5PI and you want to be facing 0.5PI but since its 0.5PI it will still go forwards even if it needs to go backward. For this, you need to put it into drive.update() and have it so that it just checks if the target is within 90 degrees of the current heading and if not reverse. It doesn't change anything with localization it works just fine.
Something is very wrong with the algo. The motor is just jittering back and forth really fast, unless I turn down the PID which then it just refuses to move it's axis. Something is likely to wrong with units and targest or something along those lines. I cannot quite figure out what.
targetSpeeds = DiffSwerveKinematics.robotToWheelVelocities(drivePower, 1.0).map {
it * constraints.maxGearVel
}.zip(listOf(0.0, 0.0))
val (leftOrientation, rightOrientation) = DiffSwerveKinematics.robotToModuleOrientations(
drivePower, 1.0
)
Here trackwidth needs to be Constraints.TrackWidth not 1.0
@amarcolini Additionally, for robotToWheelVelocities, you can't just take the quadrant and reverse. The module could be at -.5PI and you want to be facing 0.5PI but since its 0.5PI it will still go forwards even if it needs to go backward. For this, you need to put it into drive.update() and have it so that it just checks if the target is within 90 degrees of the current heading and if not reverse. It doesn't change anything with localization it works just fine.
map { it.norm() * if (sign(it.x) == 0.0) -sign(it.y) else sign(it.x) }This should work.
Something is very wrong with the algo. The motor is just jittering back and forth really fast, unless I turn down the PID which then it just refuses to move it's axis. Something is likely to wrong with units and targest or something along those lines. I cannot quite figure out what.
Could you provide the code you're using to set the drive power?
package org.firstinspires.ftc.teamcode.opmodes
import com.amarcolini.joos.command.Command
import com.amarcolini.joos.command.RobotOpMode
import com.amarcolini.joos.geometry.Pose2d
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
import org.firstinspires.ftc.teamcode.MainRobot
@TeleOp(name = "Main", group = "Main")
class MainTeleOp : RobotOpMode<MainRobot>() {
override fun preInit() {
initialize<MainRobot>()
}
override fun preStart() {
val drive = robot.drive
val driver =
Command.of {
val (leftStickX, leftStickY) = robot.gamepad.p1.getLeftStick()
val rightStickX = robot.gamepad.p1.getRightStick().x
drive.setDrivePower(Pose2d(-leftStickX, leftStickY, rightStickX) * 0.45)
}.requires(drive).onEnd { drive.setDrivePower(Pose2d(0.0, 0.0, 0.0)) }
.runUntil { robot.gamepad.p1.right_bumper.isActive }
robot.schedule(driver)
}
}@amarcolini Additionally, for robotToWheelVelocities, you can't just take the quadrant and reverse. The module could be at -.5PI and you want to be facing 0.5PI but since its 0.5PI it will still go forwards even if it needs to go backward. For this, you need to put it into drive.update() and have it so that it just checks if the target is within 90 degrees of the current heading and if not reverse. It doesn't change anything with localization it works just fine.
map { it.norm() * if (sign(it.x) == 0.0) -sign(it.y) else sign(it.x) }This should work.
That still doesn't fix the aforementioned issue where the module is at -0.5PI but since the target is at 0.5PI it still tries to go forward even though it should be going backwards
Try this:
Pose2d(-leftStickX, leftStickY, rightStickX.rad)And if the module is at -PI/2, but the target is PI/2, the module would turn to PI/2 and go forward.
Yeah, but instead the module should just stay at -PI/2 and go backward to achieve the same effect. That's why I'm suggesting the algo compare the current position and the target.
But because the target is PI/2, the module would still end up turning around. If you want the module to stay there, you would need to write a different algorithm that compares the current position to the target position and finds the shortest angle to turn to.
That's why I bound the input to -PI/2 and PI/2 for the PID controller. That's how my current algo works and it's wonderful
But the way MathUtil.wrap works, if the current angle was, say, very close to -PI/2, and the input was -PI/2, MathUtil.wrap would output PI/2, or vice versa, and the module would end up turning nearly 180 degrees. My current implementation would behave the same way, but without having to implement it in the drive class itself. The only issue that I can see happening is that for some reason, since the PID controller bounds are [-PI, PI] it goes all the way around.
Here let me send you my old ago that worked perfectly.
package org.firstinspires.ftc.teamcode.components.drive
import com.amarcolini.joos.control.FeedforwardCoefficients
import com.amarcolini.joos.control.PIDCoefficients
import com.amarcolini.joos.control.PIDFController
import com.amarcolini.joos.drive.DriveSignal
import com.amarcolini.joos.followers.HolonomicPIDVAFollower
import com.amarcolini.joos.geometry.Pose2d
import com.amarcolini.joos.geometry.Vector2d
import com.amarcolini.joos.hardware.Imu
import com.amarcolini.joos.hardware.Motor
import com.amarcolini.joos.hardware.drive.DriveComponent
import com.amarcolini.joos.localization.Localizer
import com.amarcolini.joos.trajectory.config.TrajectoryConstraints
import com.amarcolini.joos.util.wrap
import org.firstinspires.ftc.teamcode.Constants.Coefficients.FEEDFORWARD_COEFFICIENTS
import org.firstinspires.ftc.teamcode.Constants.Coefficients.HEADING_PID
import org.firstinspires.ftc.teamcode.Constants.Coefficients.MODULE_PID
import org.firstinspires.ftc.teamcode.Constants.Coefficients.TRAJECTORY_CONSTRAINTS
import org.firstinspires.ftc.teamcode.Constants.Coefficients.TRANSLATIONAL_PID
import org.firstinspires.ftc.teamcode.Constants.Module.GEAR_RATIO
import org.firstinspires.ftc.teamcode.Constants.Module.TICKS_PER_REV
import org.firstinspires.ftc.teamcode.Constants.Module.TRACK_WIDTH
import org.firstinspires.ftc.teamcode.Constants.Module.WHEEL_RADIUS
import kotlin.math.PI
import kotlin.math.abs
/**
* Differential swerve controller
* @param trackWidth the distance between the two modules
* @param translationalPID translation controller for the trajectory follower
* @param headingPID heading controller for trajectory follower
* @param modulePID PID for the swerve modules themselves
* @param feedforwardCoefficients the feedforward coeffs for the motors
* @param constraints The constraints for the drive
* @param gearRatio The gear ratio from one of the motors to the wheel shaft
* @param ticksPerRev The ticks per rev of each module
* @param wheelRadius The radius of each wheel
*/
class DifferentialSwerveDrive(
private val leftMotorA: Motor, private val leftMotorB: Motor,
private val rightMotorA: Motor, private val rightMotorB: Motor,
override val imu: Imu? = null,
// All the drive base constants
private val trackWidth: Double = TRACK_WIDTH,
private val gearRatio: Double = GEAR_RATIO,
private val ticksPerRev: Double = TICKS_PER_REV,
private val wheelRadius: Double = WHEEL_RADIUS,
// Motor Tuning
modulePID: PIDCoefficients = MODULE_PID,
feedforwardCoefficients: FeedforwardCoefficients = FEEDFORWARD_COEFFICIENTS,
// All the things to do with trajectory following
override val constraints: TrajectoryConstraints = TRAJECTORY_CONSTRAINTS,
translationalPID: PIDCoefficients = TRANSLATIONAL_PID,
headingPID: PIDCoefficients = HEADING_PID,
) : DriveComponent() {
/*
* Note that a motor A corresponds to the motor that spins the same direction as the wheel,
* and motor B spins the opposite direction.
*/
override var localizer: Localizer = DifferentialSwerveLocalizer(
::getWheelPositions, ::getWheelVelocities, ::getModuleOrientations,
trackWidth, this, imu != null
)
override val trajectoryFollower = HolonomicPIDVAFollower(
translationalPID, translationalPID,
headingPID,
Pose2d(0.5, 0.5, Math.toRadians(5.0)),
0.5
)
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(-0.5 * PI, 0.5 * PI)
rightModuleController.setInputBounds(-0.5 * PI, 0.5 * PI)
// leftModuleController.setInputBounds(-PI, PI)
// rightModuleController.setInputBounds(-PI, PI)
motors.forEach {
it.runMode = Motor.RunMode.RUN_WITHOUT_ENCODER
it.distancePerRev = 2 * PI * wheelRadius * gearRatio
it.feedforwardCoefficients = feedforwardCoefficients
it.resetEncoder()
}
}
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
fun getModuleOrientations(): Pair<Double, Double> =
(leftMotorA.currentPosition + leftMotorB.currentPosition) / ticksPerRev * PI to (rightMotorA.currentPosition + rightMotorB.currentPosition) / ticksPerRev * PI
fun getTargetModuleOrientations(): Pair<Double, Double> =
leftModuleController.targetPosition to rightModuleController.targetPosition
// private fun getDirectionalModuleOrientations(): Pair<Double, Double> {
// val (left, right) = getModuleOrientations()
// val (sameLeft, sameRight) = isSameHalf
//
// val directionalLeft = if (sameLeft) left else left + PI
// val directionalRight = if (sameRight) right else right + PI
//
// return directionalLeft to directionalRight
// }
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() {
super.update()
val moduleOrientations = getModuleOrientations()
setSpeed(leftMotorA, leftMotorB, leftModuleController, moduleOrientations.first, leftVel, leftAccel)
setSpeed(rightMotorA, rightMotorB, rightModuleController, moduleOrientations.second, rightVel, rightAccel)
motors.forEach { it.update() }
}
private fun setSpeed(
motorA: Motor,
motorB: Motor,
controller: PIDFController,
moduleOrientation: Double,
vel: Double,
accel: Double
) {
val sameHalf = isSameHalf(moduleOrientation, controller.targetPosition)
val (v, a) = calculateDirection(sameHalf, vel, accel)
motorA.setSpeed(v + controller.update(moduleOrientation), a)
motorB.setSpeed(-v + controller.update(moduleOrientation), -a)
}
/**
* This functions inverts the direction of a speed based on [sameHalf]
*/
private fun calculateDirection(
sameHalf: Boolean,
vel: Double,
accel: Double
): Pair<Double, Double> {
return if (sameHalf) {
Pair(vel, accel)
} else {
Pair(-vel, -accel)
}
}
/**
* Checks if two angles are within half pi radians of each other
*/
private fun isSameHalf(first: Double, second: Double) =
(abs(first.wrap(-PI, PI) - second.wrap(-PI, PI)) <= 0.5 * PI)
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 setRunMode(runMode: Motor.RunMode) {
motors.forEach {
it.runMode = runMode
}
}
override fun setZeroPowerBehavior(zeroPowerBehavior: Motor.ZeroPowerBehavior) {
motors.forEach {
it.zeroPowerBehavior = zeroPowerBehavior
}
}
}
Okay now I see the difference in functionality between your code and mine. What yours does is it tries its best to go in the right direction while the module is still turning, while mine just turns the wheel in the direction it should be turning even before the module reaches the correct orientation. The effect is that your code has the robot drift less while the modules are changing direction. I'll implement yours instead.
Has this issue been resolved? (OpMode throwing error on manual stop)
Yes it has :) I am still having issues with the GUI and duplicate classes.
Have you tried downloading the jar from the releases page and running that?
Just tried that.
The font is not rendering, nor is the field. (MacOS) will try windows later today.
Thanks for letting me know. I'll work on it.