diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java new file mode 100644 index 00000000..23c845bd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java @@ -0,0 +1,82 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToNote extends Command { + + private final SwerveDrive swerve; + private final Odometry odometry; + private final NoteVision vision; + + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveDriveToNote(){ + this.swerve = SwerveDrive.getInstance(); + this.odometry = Odometry.getInstance(); + this.vision = NoteVision.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) + ); + + SmartDashboard.putData("Note Detection/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + Translation2d targetTranslation = odometry.getTranslation().plus( + new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation())); + + Rotation2d targetRotation = vision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + + Pose2d targetPose = new Pose2d(targetTranslation, targetRotation); + + if (vision.hasNoteData()) { + // drive to note + swerve.setChassisSpeeds(controller.update(targetPose, odometry.getPose())); + } + else { + // only rotate toward saved note pose + swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(targetTranslation, odometry.getRotation()))); + } + + SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java new file mode 100644 index 00000000..8e254caf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -0,0 +1,72 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.NoteDetection.Rotation; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveNoteAlignedDrive extends Command { + + private final SwerveDrive swerve; + private final Odometry odometry; + private final NoteVision noteVision; + + private final VStream speed; + + private final AngleController alignController; + + public SwerveDriveNoteAlignedDrive(Gamepad driver) { + swerve = SwerveDrive.getInstance(); + odometry = Odometry.getInstance(); + noteVision = NoteVision.getInstance(); + + speed = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1.0), + x -> Settings.vpow(x, Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL), + new VLowPassFilter(Drive.RC) + ); + + alignController = new AnglePIDController(Rotation.P, Rotation.I, Rotation.D); + + addRequirements(swerve); + } + + @Override + public void execute() { + Translation2d targetTranslation = odometry.getTranslation().plus( + new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation())); + + Rotation2d targetRotation = noteVision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + + double angularVel = -alignController.update( + Angle.fromRotation2d(targetRotation), + Angle.fromRotation2d(odometry.getRotation()) + ); + + // robot relative + swerve.drive(speed.get(), angularVel); + + SmartDashboard.putNumber("Note Detection/Angle Output", alignController.getOutput()); + + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetHeading.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetHeading.java new file mode 100644 index 00000000..c14b6cda --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetHeading.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.subsystems.odometry.Odometry; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SwerveDriveResetHeading extends InstantCommand { + + public SwerveDriveResetHeading(Rotation2d heading) { + super(() -> { + Odometry o = Odometry.getInstance(); + o.reset(new Pose2d(o.getTranslation(), heading)); + }); + } + + public SwerveDriveResetHeading() { + this(new Rotation2d()); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveTranslateToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveTranslateToNote.java new file mode 100644 index 00000000..14a05ee3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveTranslateToNote.java @@ -0,0 +1,78 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveTranslateToNote extends Command { + + private final SwerveDrive swerve; + private final Odometry odometry; + private final NoteVision vision; + + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveTranslateToNote(){ + this.swerve = SwerveDrive.getInstance(); + this.odometry = Odometry.getInstance(); + this.vision = NoteVision.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) + ); + + SmartDashboard.putData("Note Detection/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + Translation2d targetTranslation = odometry.getTranslation().plus( + new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation())); + + Rotation2d targetRotation = vision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + + Pose2d targetPose = new Pose2d(targetTranslation, targetRotation); + + // translate to note only if note in view + if (vision.hasNoteData()) { + swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(odometry.getTranslation(), targetRotation))); + } + + SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java index 7d8efcd6..57f2d769 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java @@ -9,7 +9,7 @@ public class SwerveDriveXMode extends Command { // { front right, front left, back right, back left } - private static final SwerveModuleState[] states = new SwerveModuleState[] { + private static final SwerveModuleState[] states = { new SwerveModuleState(0, Rotation2d.fromDegrees(225)), new SwerveModuleState(0, Rotation2d.fromDegrees(315)), new SwerveModuleState(0, Rotation2d.fromDegrees(45)), diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveDriveToNote.java new file mode 100644 index 00000000..3796e796 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveDriveToNote.java @@ -0,0 +1,78 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve.robotrelative; + +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToNote extends Command { + + private final SwerveDrive swerve; + private final NoteVision vision; + private final Odometry odometry; + + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveDriveToNote(){ + this.swerve = SwerveDrive.getInstance(); + this.vision = NoteVision.getInstance(); + odometry = Odometry.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) + ); + + SmartDashboard.putData("Note Detection/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + Translation2d robotToNote = vision.getRobotRelativeNotePose(); + Pose2d targetPose = new Pose2d( + new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation()), + new Rotation2d()); + + if(vision.hasNoteData()) { + swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(robotToNote, robotToNote.getAngle()))); + } + else { + swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(targetPose.getTranslation(), odometry.getRotation()))); + } + + SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveTranslateToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveTranslateToNote.java new file mode 100644 index 00000000..4f45b458 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveTranslateToNote.java @@ -0,0 +1,77 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve.robotrelative; + +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveTranslateToNote extends Command { + + private final SwerveDrive swerve; + private final NoteVision vision; + private final Odometry odometry; + + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveTranslateToNote(){ + this.swerve = SwerveDrive.getInstance(); + this.vision = NoteVision.getInstance(); + odometry = Odometry.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) + ); + + SmartDashboard.putData("Note Detection/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + Translation2d robotToNote = vision.getRobotRelativeNotePose(); + Rotation2d kZero = new Rotation2d(); + Pose2d targetPose = new Pose2d( + new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation()), + kZero); + + // translate to note only if note in view + if(vision.hasNoteData()) { + swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(robotToNote, kZero))); + } + + SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 3cbedd38..304376f1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -96,6 +96,7 @@ public interface Swerve { // between wheel centers double WIDTH = Units.inchesToMeters(20.75); double LENGTH = Units.inchesToMeters(20.75); + double CENTER_TO_INTAKE_FRONT = Units.inchesToMeters(18); // TODO: redetermine for izzi (from reteP) SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Max Module Speed (meter per s)", 5.0); SmartNumber MAX_TURNING = new SmartNumber("Swerve/Max Turn Velocity (rad per s)", 6.28); diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index 1e0101fa..0a1c2098 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -9,6 +9,8 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -86,6 +88,22 @@ public Pose2d getPose() { return estimator.getEstimatedPosition(); } + /** + * Returns the translation of the robot. + * @return the translation of the robot + */ + public Translation2d getTranslation() { + return getPose().getTranslation(); + } + + /** + * Returns the rotation of the robot. + * @return the rotation of the robot + */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + /** * Reset the pose of the odometry to the given pose. * @param pose the pose to reset to diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java index fb73dae1..a8f158de 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java @@ -37,7 +37,7 @@ protected LLNoteVision() { } /** - * Returns whether the Limelight has data. + * Get whether the Limelight has data. * @return whether the Limelight has data */ @Override @@ -51,7 +51,7 @@ public boolean hasNoteData() { } /** - * Returns the estimated pose of the note. + * Get the estimated pose of the note. * @return the estimated pose of the note */ @Override @@ -59,8 +59,20 @@ public Translation2d getEstimatedNotePose() { return notePose; } + @Override + public Translation2d getRobotRelativeNotePose() { + Translation2d sum = new Translation2d(); + + for (Limelight limelight : limelights) { + sum = sum.plus(new Translation2d(limelight.getDistanceToNote(), Rotation2d.fromDegrees(limelight.getXAngle()))); + } + + return sum.div(limelights.length); + } + /** - * Calculates the estimated pose of the note. + * @Calculates the estimated pose of the note by averaging data from all available limelights. + * Sets the pose to `notePose` that can be accessed with `getEstimatedNotePose()`. */ private void updateNotePose() { Translation2d sum = new Translation2d(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java index 6cf7e12b..b36559c9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java @@ -1,5 +1,6 @@ package com.stuypulse.robot.subsystems.vision; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -18,5 +19,10 @@ public static NoteVision getInstance() { public abstract boolean hasNoteData(); public abstract Translation2d getEstimatedNotePose(); + public abstract Translation2d getRobotRelativeNotePose(); + + public final Rotation2d getRotationToNote() { + return getRobotRelativeNotePose().getAngle(); + } } diff --git a/src/main/java/com/stuypulse/robot/util/vision/Limelight.java b/src/main/java/com/stuypulse/robot/util/vision/Limelight.java index 018b466a..11016c08 100644 --- a/src/main/java/com/stuypulse/robot/util/vision/Limelight.java +++ b/src/main/java/com/stuypulse/robot/util/vision/Limelight.java @@ -100,7 +100,11 @@ public double getYAngle() { return tyData - Units.radiansToDegrees(POSITIONS[limelightID].getRotation().getY()); } - // limelight targets far end of note, so have to subtract half of note length + /** + * Calculates the distance from the robot's center to the note's center. + * Limelight targets far end of note, so half of note length is substracted. + * @return distance from robot center to note center. + */ public double getDistanceToNote() { Rotation2d yRotation = Rotation2d.fromDegrees(getYAngle()); return POSITIONS[limelightID].getZ() / -yRotation.getTan() + POSITIONS[limelightID].getX() - Field.NOTE_LENGTH / 2.0;