Coaxial Swerve Config Example
package org.firstinspires.ftc.teamcode.pedro;
import com.pedropathing.VectorCalculator;
import com.pedropathing.control.FilteredPIDFCoefficients;
import com.pedropathing.control.PIDFCoefficients;
import com.pedropathing.control.PredictiveBrakingCoefficients;
import com.pedropathing.follower.Follower;
import com.pedropathing.follower.FollowerConstants;
import com.pedropathing.ftc.FollowerBuilder;
import com.pedropathing.ftc.drivetrains.CoaxialPod;
import com.pedropathing.ftc.drivetrains.SwerveConstants;
import com.pedropathing.ftc.localization.constants.PinpointConstants;
import com.pedropathing.geometry.Pose;
import me.munchem.electrodromos.Dromos;
import me.munchem.electrodromos.DromosConfig;
import com.pedropathing.paths.PathConstraints;
import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class PedroConstants {
public static CoaxialPodConfig leftFront = new CoaxialPodConfig(
c -> {
c.name.set("leftFront");
c.motorName.set("lf");
c.servoName.set("lfTurn");
c.servoEncoderName.set("lfTurnEncoder");
c.turnController.set(Controller.pid(0.3, 0, 0.0086)
.plus(Controller.proportionalFeedforward(0.0130)));
c.driveDirection.set(DcMotorSimple.Direction.REVERSE);
c.servoDirection.set(DcMotorSimple.Direction.FORWARD);
c.angleOffsetRad.set(Math.toRadians(0));
c.podOffset.set(Vector2D.cartesian(-6, 6));
}
);
public static CoaxialPodConfig rightFront = new CoaxialPodConfig(
c -> {
c.name.set("rightFront");
c.motorName.set("rf");
c.servoName.set("rfTurn");
c.servoEncoderName.set("rfTurnEncoder");
c.turnController.set(Controller.pid(0.3, 0, 0.0086)
.plus(Controller.proportionalFeedforward(0.0130)));
c.driveDirection.set(DcMotorSimple.Direction.FORWARD);
c.servoDirection.set(DcMotorSimple.Direction.FORWARD);
c.angleOffsetRad.set(Math.toRadians(0));
c.podOffset.set(Vector2D.cartesian(6, 6));
}
);
public static CoaxialPodConfig leftBack = new CoaxialPodConfig(
c -> {
c.name.set("leftBack");
c.motorName.set("lb");
c.servoName.set("lbTurn");
c.servoEncoderName.set("lbTurnEncoder");
c.turnController.set(Controller.pid(0.3, 0, 0.0086)
.plus(Controller.proportionalFeedforward(0.0190)));
c.driveDirection.set(DcMotorSimple.Direction.REVERSE);
c.servoDirection.set(DcMotorSimple.Direction.FORWARD);
c.angleOffsetRad.set(Math.toRadians(0));
c.podOffset.set(Vector2D.cartesian(-6, -6));
}
);
public static CoaxialPodConfig rightBack = new CoaxialPodConfig(
c -> {
c.name.set("rightBack");
c.motorName.set("rb");
c.servoName.set("rbTurn");
c.servoEncoderName.set("rbTurnEncoder");
c.turnController.set(Controller.pid(0.3, 0, 0.0086)
.plus(Controller.proportionalFeedforward(0.0190)));
c.driveDirection.set(DcMotorSimple.Direction.FORWARD);
c.servoDirection.set(DcMotorSimple.Direction.FORWARD);
c.angleOffsetRad.set(Math.toRadians(0));
c.podOffset.set(Vector2D.cartesian(6, -6));
}
);
public static SwerveConfig driveConfig = new SwerveConfig(
c -> {
c.zeroPowerBehavior.set(SwerveConfig.ZeroPowerBehavior.X_LOCK);
c.manualBrakeMode.set(true);
c.voltageCompensation.set(false);
}
);
// ElectroDromos. Pedro's SwerveConfig.voltageCompensation stays false above, so only one of them compensates.
public static DromosConfig dromosConfig = new DromosConfig(
c -> {
c.voltageCompensation.set(true);
c.nominalVoltage.set(12.62); // from the Voltage Compensation Tuner
}
);
public static Follower create(HardwareMap h) {
CoaxialPod leftFrontPod = new CoaxialPod(h, leftFront);
CoaxialPod rightFrontPod = new CoaxialPod(h, rightFront);
CoaxialPod leftBackPod = new CoaxialPod(h, leftBack);
CoaxialPod rightBackPod = new CoaxialPod(h, rightBack);
Dromos dromos = new Dromos(h, dromosConfig);
return new Follower(
dromos.localizer(new PinpointLocalizer(h, localizerConfig)),
dromos.drivetrain(new Swerve(h, driveConfig,
leftBackPod, leftFrontPod, rightBackPod, rightFrontPod)),
new Foresight(foresightConfig));
}
}One voltage compensation, not two
Pedro's Swerve has its own voltageCompensation option in SwerveConfig. Turn on either Pedro's or ElectroDromos's, never both.
If both are on, dromos.drivetrain(...) throws an IllegalStateException at init, so it can't scale power twice by accident.
ElectroDromos's only scales path following and holding (never manual driving), and comes with a tuner for nominalVoltage.
Last updated on