/* * Copyright (c) 2023 Titan Robotics Club (http://www.titanrobotics.com) * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in all * copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * SOFTWARE. */ package teamcode.drivebases; import java.io.FileNotFoundException; import java.io.FileOutputStream; import java.io.FileReader; import java.io.PrintStream; import java.util.Arrays; import java.util.Scanner; import ftclib.motor.FtcCRServo; import ftclib.motor.FtcDcMotor; import ftclib.sensor.FtcAnalogEncoder; import teamcode.RobotParams; import trclib.drivebase.TrcSwerveDriveBase; import trclib.drivebase.TrcSwerveModule; import trclib.path.TrcPidDrive; import trclib.path.TrcPurePursuitDrive; import trclib.robotcore.TrcDbgTrace; import trclib.sensor.TrcOdometryWheels; /** * This class creates the RobotDrive subsystem that consists of wheel motors and related objects for driving the * robot. */ public class SwerveDrive extends RobotDrive { private static final String moduleName = SwerveDrive.class.getSimpleName(); private static final TrcDbgTrace globalTracer = TrcDbgTrace.getGlobalTracer(); private final String[] steerEncoderNames = { RobotParams.HWNAME_LFSTEER_ENCODER, RobotParams.HWNAME_RFSTEER_ENCODER, RobotParams.HWNAME_LBSTEER_ENCODER, RobotParams.HWNAME_RBSTEER_ENCODER}; public double[] zeroPositions = { RobotParams.LFSTEER_ZERO_POS, RobotParams.RFSTEER_ZERO_POS, RobotParams.LBSTEER_ZERO_POS, RobotParams.RBSTEER_ZERO_POS}; private final String[] steerServoNames = { RobotParams.HWNAME_LFSTEER_SERVO, RobotParams.HWNAME_RFSTEER_SERVO, RobotParams.HWNAME_LBSTEER_SERVO, RobotParams.HWNAME_RBSTEER_SERVO}; private final boolean[] steerServoInverted = { RobotParams.LFSTEER_INVERTED, RobotParams.RFSTEER_INVERTED, RobotParams.LBSTEER_INVERTED, RobotParams.RBSTEER_INVERTED}; private final String[] swerveModuleNames = {"lfSwerveModule", "rfSwerveModule", "lbSwerveModule", "rbSwerveModule"}; // // Swerve steering motors and modules. // public final FtcAnalogEncoder[] steerEncoders; public final FtcCRServo[] steerServos; public final TrcSwerveModule[] swerveModules; public int calibrationCount = 0; private String xModeOwner = null; /** * Constructor: Create an instance of the object. */ public SwerveDrive() { super(); readSteeringCalibrationData(); driveMotors = createDriveMotors(driveMotorNames, driveMotorInverted); steerEncoders = createSteerEncoders( steerEncoderNames, RobotParams.Preferences.doSwervePhysicalAlignment ? new double[] {0.0, 0.0, 0.0, 0.0} : zeroPositions); steerServos = createSteerServos(steerServoNames, steerServoInverted, steerEncoders); swerveModules = createSwerveModules(swerveModuleNames, driveMotors, steerServos); driveBase = new TrcSwerveDriveBase( swerveModules[INDEX_LEFT_FRONT], swerveModules[INDEX_LEFT_BACK], swerveModules[INDEX_RIGHT_FRONT], swerveModules[INDEX_RIGHT_BACK], gyro, RobotParams.DRIVE_BASE_WIDTH, RobotParams.DRIVE_BASE_LENGTH); driveBase.setDriveOrientation(RobotParams.DEF_DRIVE_ORIENTATION, true); if (RobotParams.Preferences.useExternalOdometry) { // // Create the external odometry device that uses the right back encoder port as the X odometry and // the left and right front encoder ports as the Y1 and Y2 odometry. Gyro will serve as the angle // odometry. // TrcOdometryWheels driveBaseOdometry = new TrcOdometryWheels( new TrcOdometryWheels.AxisSensor( driveMotors[INDEX_RIGHT_BACK], RobotParams.X_ODWHEEL_Y_OFFSET, RobotParams.X_ODWHEEL_X_OFFSET), new TrcOdometryWheels.AxisSensor[] { new TrcOdometryWheels.AxisSensor( driveMotors[INDEX_LEFT_FRONT], RobotParams.YLEFT_ODWHEEL_X_OFFSET, RobotParams.YLEFT_ODWHEEL_Y_OFFSET), new TrcOdometryWheels.AxisSensor( driveMotors[INDEX_RIGHT_FRONT], RobotParams.YRIGHT_ODWHEEL_X_OFFSET, RobotParams.YRIGHT_ODWHEEL_Y_OFFSET)}, gyro); // // Set the drive base to use the external odometry device overriding the built-in one. // driveBase.setDriveBaseOdometry(driveBaseOdometry); driveBase.setOdometryScales(RobotParams.ODWHEEL_INCHES_PER_COUNT, RobotParams.ODWHEEL_INCHES_PER_COUNT); } else { driveBase.setOdometryScales(RobotParams.YPOS_INCHES_PER_COUNT, RobotParams.YPOS_INCHES_PER_COUNT); } // // Create and initialize PID controllers. // pidDrive = new TrcPidDrive( "pidDrive", driveBase, RobotParams.xPosPidCoeff, RobotParams.XPOS_TOLERANCE, driveBase::getXPosition, RobotParams.yPosPidCoeff, RobotParams.YPOS_TOLERANCE, driveBase::getYPosition, RobotParams.turnPidCoeff, RobotParams.TURN_TOLERANCE, driveBase::getHeading); pidDrive.getXPidCtrl().setRampRate(RobotParams.X_RAMP_RATE); pidDrive.getYPidCtrl().setRampRate(RobotParams.Y_RAMP_RATE); pidDrive.getTurnPidCtrl().setRampRate(RobotParams.TURN_RAMP_RATE); pidDrive.getTurnPidCtrl().setAbsoluteSetPoint(true); // FTC robots generally have USB performance issues where the sampling rate of the gyro is not high enough. // If the robot turns too fast, PID will cause oscillation. By limiting turn power, the robot turns slower. pidDrive.getTurnPidCtrl().setOutputLimit(RobotParams.TURN_POWER_LIMIT); // AbsoluteTargetMode eliminates cumulative errors on multi-segment runs because drive base is keeping track // of the absolute target position. pidDrive.setAbsoluteTargetModeEnabled(true); pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, false, false, false); purePursuitDrive = new TrcPurePursuitDrive( "purePursuitDrive", driveBase, RobotParams.PPD_FOLLOWING_DISTANCE, RobotParams.PPD_POS_TOLERANCE, RobotParams.PPD_TURN_TOLERANCE, RobotParams.xPosPidCoeff, RobotParams.yPosPidCoeff, RobotParams.turnPidCoeff, RobotParams.velPidCoeff); purePursuitDrive.setFastModeEnabled(true); purePursuitDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, false, false, false); } //SwerveDrive /** * This method creates and configures all steer encoders. * * @param encoderNames specifies an array of names for each steer encoder. * @param zeroOffsets specifies an array of zero offsets for each steer encoder. * @return an array of created steer encoders. */ private FtcAnalogEncoder[] createSteerEncoders(String[] encoderNames, double[] zeroOffsets) { FtcAnalogEncoder[] encoders = new FtcAnalogEncoder[encoderNames.length]; for (int i = 0; i < steerEncoderNames.length; i++) { encoders[i] = new FtcAnalogEncoder(encoderNames[i]); encoders[i].setScaleAndOffset(180.0, 0.0, zeroOffsets[i]); // Enable Cartesian converter. encoders[i].setEnabled(true); } return encoders; } //createSteerEncoders /** * This method creates and configures all steer servos. * * @param servoNames specifies an array of names for each steer servo. * @param inverted specifies an array of boolean indicating if the servo needs to be inverted. * @param encoders specifies an array of encoders for each steer servo. * @return an array of created steer servos. */ private FtcCRServo[] createSteerServos(String[] servoNames, boolean[] inverted, FtcAnalogEncoder[] encoders) { FtcCRServo[] servos = new FtcCRServo[servoNames.length]; for (int i = 0; i < servoNames.length; i++) { servos[i] = new FtcCRServo(servoNames[i], encoders[i]); if (RobotParams.Preferences.swerveDualServoSteering) { FtcCRServo slaveServo = new FtcCRServo(servoNames[i] + ".slave", null); slaveServo.follow(servos[i], false); } servos[i].setMotorInverted(inverted[i]); servos[i].setSoftwarePidEnabled(true); servos[i].setPositionPidParameters( RobotParams.STEER_SERVO_KP, RobotParams.STEER_SERVO_KI, RobotParams.STEER_SERVO_KD, RobotParams.STEER_SERVO_KF, RobotParams.STEER_SERVO_IZONE, RobotParams.STEER_SERVO_TOLERANCE); } return servos; } //createSteerServos /** * This method creates and configures all swerve modules. * * @param moduleNames specifies an array of names for each swerve module.needs to be inverted. * @param driveMotors specifies an array of drive motors. * @param steerServos specifies an array of steer servos. * @return an array of created swerve modules. */ private TrcSwerveModule[] createSwerveModules( String[] moduleNames, FtcDcMotor[] driveMotors, FtcCRServo[] steerServos) { TrcSwerveModule[] modules = new TrcSwerveModule[moduleNames.length]; for (int i = 0; i < moduleNames.length; i++) { modules[i] = new TrcSwerveModule(moduleNames[i], driveMotors[i], steerServos[i]); } return modules; } //createSwerveModules /** * This method enables/disables performance monitoring of all steering servo motors. * * @param enabled specifies true to enable, false to disable. */ public void setSteerPerformanceMonitorEnabled(boolean enabled) { for (FtcCRServo servo: steerServos) { servo.setPerformanceMonitorEnabled(enabled); } } //setSteerPerformanceMonitorEnabled /** * This method prints the performance info to the trace log. */ public void printSteerPerformanceInfo() { for (FtcCRServo servo : steerServos) { servo.printPidControlTaskPerformance(); } } //printSteerPerformanceInfo /** * This method sets the steering angle of all swerve modules. * * @param angle specifies the steer angle. * @param optimize specifies true to optimize (only turns within +/- 90 degrees), false otherwse. * @param hold specifies true to hold the angle, false otherwise. */ public void setSteerAngle(double angle, boolean optimize, boolean hold) { for (TrcSwerveModule module: swerveModules) { module.setSteerAngle(angle, optimize, hold); } } //setSteerAngle /** * This method set all the wheels into an X configuration so that nobody can bump us out of position. If owner * is specifies, it will acquire execlusive ownership of the drivebase on behalf of the specified owner. On * disable, it will release the ownership. * * @param owner specifies the ID string of the caller for checking ownership, can be null if caller is not * ownership aware. * @param enabled specifies true to enable anti-defense mode, false to disable. */ public void setXModeEnabled(String owner, boolean enabled) { if (enabled) { if (owner != null && !driveBase.hasOwnership(owner) && driveBase.acquireExclusiveAccess(owner)) { xModeOwner = owner; } ((TrcSwerveDriveBase) driveBase).setXMode(owner); } else if (xModeOwner != null) { driveBase.releaseExclusiveAccess(xModeOwner); xModeOwner = null; } } //setXModeEnabled /** * This method starts the steering calibration. */ public void startSteeringCalibration() { calibrationCount = 0; Arrays.fill(zeroPositions, 0.0); } //startSteeringCalibration /** * This method stops the steering calibration and saves the calibration data to a file. */ public void stopSteeringCalibration() { for (int i = 0; i < zeroPositions.length; i++) { zeroPositions[i] /= calibrationCount; } calibrationCount = 0; saveSteeringCalibrationData(); } //stopSteeringCalibration /** * This method is called periodically to sample the steer encoders for averaging the zero position data. */ public void runSteeringCalibration() { for (int i = 0; i < zeroPositions.length; i++) { zeroPositions[i] += steerEncoders[i].getRawPosition(); } calibrationCount++; } //runSteeringCalibration /** * This method saves the calibration data to a file on the Robot Controller. */ public void saveSteeringCalibrationData() { try (PrintStream out = new PrintStream(new FileOutputStream( RobotParams.TEAM_FOLDER_PATH + "/" + RobotParams.STEERING_CALIBRATION_DATA_FILE))) { for (int i = 0; i < steerServoNames.length; i++) { out.printf("%s: %f\n", steerServoNames[i], zeroPositions[i]); } out.close(); globalTracer.traceInfo( moduleName, "SteeringCalibrationData%s=%s", Arrays.toString(steerServoNames), Arrays.toString(zeroPositions)); } catch (FileNotFoundException e) { e.printStackTrace(); } } //saveSteeringCalibrationData /** * This method reads the steering calibration data from a file on the Robot Controller. * * @throws RuntimeException if file contains invalid data. */ public void readSteeringCalibrationData() { String line = null; try (Scanner in = new Scanner(new FileReader( RobotParams.TEAM_FOLDER_PATH + "/" + RobotParams.STEERING_CALIBRATION_DATA_FILE))) { for (int i = 0; i < steerServoNames.length; i++) { line = in.nextLine(); int colonPos = line.indexOf(':'); String name = colonPos == -1? null: line.substring(0, colonPos); if (name == null || !name.equals(steerServoNames[i])) { throw new RuntimeException("Invalid servo name in line " + line); } zeroPositions[i] = Double.parseDouble(line.substring(colonPos + 1)); } } catch (FileNotFoundException e) { globalTracer.traceWarn(moduleName, "Steering calibration data file not found, using built-in defaults."); } catch (NumberFormatException e) { globalTracer.traceErr(moduleName, "Invalid zero position value in line %s", line); } catch (RuntimeException e) { globalTracer.traceErr(moduleName, "Invalid servo name in line %s", line); } globalTracer.traceInfo( moduleName, "SteeringCalibrationData%s=%s", Arrays.toString(steerServoNames), Arrays.toString(zeroPositions)); } //readSteeringCalibrationData } //class SwerveDrive