Doing some experiments with clock drive on the swerve. I don’t have a complete drive, but have worked out a Command for it. More or less.
Excuse the crudity of this model. I didn’t have time to paint it or make it to scale.
Any feedback would be appreciated.
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems.drive;
import java.util.function.DoubleSupplier;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class SwerveDrive extends SubsystemBase {
private final ProfiledPIDController rotationController = new ProfiledPIDController(
0, 0, 0, new Constraints(0, 0));
private final SimpleMotorFeedforward feeforward = new SimpleMotorFeedforward(0, 0, 0);
public SwerveDrive() {
}
private Pose2d getPosition() {
return null;
}
private double getAngularSpeed() {
return 0.0;
}
private void setFieldChassisSpeed(ChassisSpeeds speeds) {
// Does the rest with swervemodule funnctions and stuff.
}
public Command clockDrive(
DoubleSupplier forward,
DoubleSupplier strafe,
DoubleSupplier rotateY,
DoubleSupplier rotateX) {
return run(() -> {
double f = -forward.getAsDouble();
double s = -strafe.getAsDouble();
double y = -rotateY.getAsDouble();
double x = -rotateX.getAsDouble();
double t = 0.0;
if (x == 0 && y == 0) {
t = getPosition().getRotation().getRadians();
} else {
t = Math.atan2(y, x);
}
double tcurrent = getPosition().getRotation().getRadians();
double wcurrent = getAngularSpeed();
double w = rotationController.calculate(tcurrent, t);
// not sure about feed forward.
w += feeforward.calculate(wcurrent, rotationController.getSetpoint().position, 0.020);
ChassisSpeeds speeds = new ChassisSpeeds(f, s, w);
setFieldChassisSpeed(speeds);
});
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
So the idea here would be using the one joystick for field centric forward and strafe and the other one for the robot angle field centric perspective. Setting the swerve drive angle by position instead of directly giving it an angular velocity.
So the robot rotation is " clocked" to a positional control on a controller?
e.g. if i had a potentiometer on a control panel at 0 degrees to robot is at zero degrees and forward strafe command is forward on the field. And if the pot is at 90 degrees the front of the bot is at 90 degrees the forward strafe command is forward on the field?
That makes sense to me. I believe what you wrote does work as intended.
In my opinion a better option for swerve control would be to have heading snap. So basically close the loop on whatever your current angle is, and adjust when you rotate the stick
I think @nuclearnerd will have some opinions based on the steering knob experiments. (Although I think those were more of a tactile thing and not controlling the direct position)
Your solution may make sense for some things, not better or worse necessarily, just different.
I agree with the heading snap idea if you need some specific values tho (i.e. align to a field element)… Press button robot goes to needed heading. Less mental load on the driver to accomplish the same thing.
So the rotation joystick x axis works to control angular velocity directly. But when you let go of the stick, a heading PID controller takes over. It’s set position would just be whatever the current angle of the robot is.
The reason I think this is better is because on most vehicles you are directly or indirectly controlling the power that goes into something, not the direct position. I think it’s more intuitive to not control position directly
I believe this functionality is already built into YAGSL, although I don’t know what the mode is called.
I think position control of steering is equally as intuitive as rate control, but our testing suggested driver performance was no better with position control.
I was wondering how well this would work against defence.
One tactic I have been using continuously as a driver to evade defence was spinning around the enemy robot until it is passed
It seems to me that this maneuver will be unnecessary complicated by using “clock drive”
(Compare just holding the joystick in place against constantly having to rotate it)
Anyway would love to hear how well your drivers adjusted to the change
Agreed. I think if you had position control for rotation, you’d still want additional buttons for “spin fast left / spin fast right” for scenarios like evading defense.
However, using “clockdrive” positional control, it should be easier to drive an arc around an object while retaining a robot heading pointed at that object (e.g. 2022 hub, 2018 switch). At least compared to rate-based control.
(Of course you can have driver aids and telemetry do this as well, especially with vision)
