Skip to content

Commit

Permalink
trap source alignment stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
@amb2127
amb2127 committed Mar 6, 2024
1 parent bd48307 commit 9be41db
Show file tree
Hide file tree
Showing 11 changed files with 239 additions and 26 deletions.
2 changes: 1 addition & 1 deletion FRC-2024.chor
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
"controlIntervalCount": 30
},
{
"x": 7.615901470184326,
"x": 7.5159014701843265,
"y": 7.490461349487305,
"heading": 0,
"isInitialGuess": false,
Expand Down
1 change: 1 addition & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
i need help
18 changes: 17 additions & 1 deletion src/main/java/org/codeorange/frc2024/robot/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
package org.codeorange.frc2024.robot;

import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
Expand Down Expand Up @@ -220,7 +222,7 @@ public static class Ports {
public static final double SS_DEPLOYCLIMBER2_ARM = isPrototype() ? 0 : 0;
public static final double SS_DEPLOYCLIMBER2_WRIST = isPrototype() ? 0 : 0;
public static final double SS_DEPLOYCLIMBER2_CLIMBER = isPrototype() ? CLIMBER_UPPER_LIMIT_ROTATIONS : 0;
public static final double SS_CLIMB_ELEVATOR = isPrototype() ? 0 : 16;
public static final double SS_CLIMB_ELEVATOR = isPrototype() ? 0 : 18;
public static final double SS_CLIMB_ARM = isPrototype() ? 0 : 0.205555;
public static final double SS_CLIMB_WRIST = isPrototype() ? 0 : 0;
public static final double SS_CLIMB_CLIMBER = isPrototype() ? 0 : 0;
Expand Down Expand Up @@ -354,4 +356,18 @@ public static class AngleLookupInterpolation {
}
}


public static final Translation2d BLUE_STAGE_CENTER =
new Translation2d(
4.904,
4.107
);

public static final Translation2d RED_STAGE_CENTER =
new Translation2d(
11.678,
4.108
);

public static final double CLIMBER_SWITCH_OFFSET = 2.89 - 0.95 / CLIMBER_P;
}
147 changes: 134 additions & 13 deletions src/main/java/org/codeorange/frc2024/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,13 @@

import com.choreo.lib.ChoreoTrajectory;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
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.units.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import org.codeorange.frc2024.auto.AutoManager;
import org.codeorange.frc2024.subsystem.AbstractSubsystem;
import org.codeorange.frc2024.subsystem.arm.*;
Expand Down Expand Up @@ -213,9 +212,8 @@ public void robotInit() {

superstructure.start();

if(!isCompetition() || climber.limitSwitchPushed()) {
superstructure.setCurrentState(Superstructure.States.STOW);
}
superstructure.setCurrentState(Superstructure.States.STOW);


AutoManager.getInstance();
AutoLogOutputManager.addPackage("org.codeorange.frc2024.subsystem");
Expand Down Expand Up @@ -274,12 +272,12 @@ public void teleopInit() {
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {
if(limelightLEDchooser.get()) {
LimelightHelpers.setLEDMode_PipelineControl("limelight-front");
LimelightHelpers.setLEDMode_PipelineControl("limelight-back");
} else {
if(Math.hypot(drive.getChassisSpeeds().vxMetersPerSecond, drive.getChassisSpeeds().vyMetersPerSecond) > 1 || !limelightLEDchooser.get()) {
LimelightHelpers.setLEDMode_ForceOff("limelight-front");
LimelightHelpers.setLEDMode_ForceOff("limelight-back");
} else {
LimelightHelpers.setLEDMode_PipelineControl("limelight-front");
LimelightHelpers.setLEDMode_PipelineControl("limelight-back");
}

if(!prevHasNote && intake.hasNote()) {
Expand Down Expand Up @@ -385,12 +383,21 @@ public void teleopPeriodic() {
}
}

superstructure.a = MathUtil.applyDeadband(-logitechThing.getRawAxis(1), 0.1);
superstructure.shotWristdelta = MathUtil.applyDeadband(-logitechThing.getRawAxis(1), 0.1);

if(superstructure.getCurrentState() == Superstructure.States.PUPPETEERING) {
superstructure.wantedPuppeteerWrist += (MathUtil.applyDeadband(-logitechThing.getRawAxis(1), 0.1))/360;
superstructure.wantedPuppeteerArm += (MathUtil.applyDeadband(-logitechThing.getRawAxis(0), 0.1))/360;
superstructure.wantedPuppeteerElevator += (MathUtil.applyDeadband(logitechThing.getRawAxis(2), 0.1))/50;
}

if(logitechThing.getRawButton(4) && logitechThing.getRawButton(6)) {
superstructure.setGoalState(Superstructure.States.PUPPETEERING);
}
ControllerDriveInputs controllerDriveInputs = getControllerDriveInputs();
if(xbox.getRawAxis(Controller.XboxAxes.LEFT_TRIGGER) > 0.1) {
drive.swerveDriveTargetAngle(controllerDriveInputs, drive.findAngleToSpeaker());
} else if(xbox.getRawButton(XboxButtons.X)) {
} else if(xbox.getRawButton(XboxButtons.Y)) {
double targetAngle;

if(intake.hasNote()) {
Expand All @@ -404,6 +411,51 @@ public void teleopPeriodic() {
}

drive.swerveDriveTargetAngle(controllerDriveInputs, targetAngle);
} else if(xbox.getRawButton(XboxButtons.X)) {
var drivePose = drive.getPose();
double rotationFromStage;
Translation2d stageCenter;
StageSpots selectedSpot;

if(isRed()) {
stageCenter = RED_STAGE_CENTER;
rotationFromStage = Math.atan2(drivePose.getY() - stageCenter.getY(), drivePose.getX() - stageCenter.getX());
if(rotationFromStage >= 2 * Math.PI / 3 || rotationFromStage <= -2 * Math.PI / 3) {
selectedSpot = StageSpots.TOWARDS_CENTER;
} else if(rotationFromStage >= 0) {
selectedSpot = StageSpots.TOWARDS_AMP;
} else {
selectedSpot = StageSpots.TOWARDS_SOURCE;
}
} else {
stageCenter = BLUE_STAGE_CENTER;
rotationFromStage = Math.atan2(drivePose.getY() - stageCenter.getY(), drivePose.getX() - stageCenter.getX());
if(rotationFromStage <= Math.PI / 3 && rotationFromStage >= -Math.PI / 3) {
selectedSpot = StageSpots.TOWARDS_CENTER;
} else if(rotationFromStage >= Math.PI / 3) {
selectedSpot = StageSpots.TOWARDS_AMP;
} else {
selectedSpot = StageSpots.TOWARDS_SOURCE;
}
}

Pose2d wantedPose;

if(superstructure.getCurrentState() == Superstructure.States.CLIMBER) {
if(isRed()) {
wantedPose = selectedSpot.redClimbPose;
} else {
wantedPose = selectedSpot.blueClimbPose;
}
} else {
if(isRed()) {
wantedPose = selectedSpot.redTrapPose;
} else {
wantedPose = selectedSpot.blueTrapPose;
}
}

drive.driveTargetPose(wantedPose);
} else {
drive.drive(controllerDriveInputs, true, true);
}
Expand Down Expand Up @@ -439,6 +491,75 @@ public void disabledInit() {
drive.setBrakeMode(true);
}

public enum StageSpots {
TOWARDS_CENTER(
new Pose2d(
new Translation2d(10.406, 4.150),
Rotation2d.fromDegrees(0)
),
new Pose2d(
new Translation2d(6.187, 4.0),
Rotation2d.fromDegrees(180)
),
new Pose2d(
new Translation2d(10.406, 4.150),
Rotation2d.fromDegrees(0)
),
new Pose2d(
new Translation2d(6.187, 4.0),
Rotation2d.fromDegrees(180)
)
),
TOWARDS_AMP(
new Pose2d(
new Translation2d(12.488, 5.222),
Rotation2d.fromDegrees(-120)
),
new Pose2d(
new Translation2d(4.387, 5.410),
Rotation2d.fromDegrees(-60)
),
new Pose2d(
new Translation2d(12.488, 5.222),
Rotation2d.fromDegrees(-120)
),
new Pose2d(
new Translation2d(4.387, 5.410),
Rotation2d.fromDegrees(-60)
)
),
TOWARDS_SOURCE(
new Pose2d(
new Translation2d(12.227, 2.880),
Rotation2d.fromDegrees(120)
),
new Pose2d(
new Translation2d(4.073, 3.015),
Rotation2d.fromDegrees(60)
),
new Pose2d(
new Translation2d(12.121, 3.320),
Rotation2d.fromDegrees(120)
),
new Pose2d(
new Translation2d(4.073, 3.015),
Rotation2d.fromDegrees(60)
)
);

private final Pose2d redTrapPose;
private final Pose2d blueTrapPose;
private final Pose2d redClimbPose;
private final Pose2d blueClimbPose;

StageSpots(Pose2d redTrapPose, Pose2d blueTrapPose, Pose2d redClimbPose, Pose2d blueClimbPose) {
this.redTrapPose = redTrapPose;
this.redClimbPose = redClimbPose;
this.blueTrapPose = blueTrapPose;
this.blueClimbPose = blueClimbPose;
}
}

/** This function is called periodically when disabled. */
@Override
public void disabledPeriodic() {
Expand Down
36 changes: 33 additions & 3 deletions src/main/java/org/codeorange/frc2024/subsystem/Superstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ public class Superstructure extends AbstractSubsystem {
private static Climber climber;
//private static Vision vision;
private States currentState = States.STOW;
@AutoLogOutput(key = "Superstructure/Goal State")
private States goalState = States.STOW;
public boolean isFlipped = false;
public boolean climberOut = false;
Expand Down Expand Up @@ -209,6 +210,10 @@ public void update() {
if(climber.getPositionInRotations() > 184) {
superstructure.climberOut = true;
}

if(superstructure.goalState == States.PUPPETEERING) {
superstructure.setCurrentState(States.PUPPETEERING);
}
}
}
},
Expand Down Expand Up @@ -249,6 +254,14 @@ public void update() {
superstructure.setCurrentState(superstructure.goalState);
}
}
},
PUPPETEERING(0, 0, 0) {
@Override
public void update() {
if(superstructure.goalState != States.PUPPETEERING) {
superstructure.setCurrentState(States.INTERMEDIATE);
}
}
};
@AutoLogOutput(key = "Superstructure/Is At Wanted State")
public boolean isAtWantedState() {
Expand Down Expand Up @@ -286,13 +299,29 @@ public void setCurrentState(States newState) {
private double wantedShooterPosition;

@AutoLogOutput(key = "Axis Add")
public double a;
public double shotWristdelta;

public double wantedPuppeteerArm = 0;
public double wantedPuppeteerWrist = 0;
public double wantedPuppeteerElevator = 0;

public final double podium_front = 34;
public final double podium_back = 0;

private States prevState;

public void update() {
if(!DriverStation.isTest()) {
if(prevState != currentState && currentState == States.PUPPETEERING) {
wantedPuppeteerArm = prevState.armPos;
wantedPuppeteerWrist = prevState.wristPos;
wantedPuppeteerElevator = prevState.elevatorPos;
}

if(currentState == States.PUPPETEERING) {
arm.setPosition(wantedPuppeteerArm);
wrist.setWristPosition(wantedPuppeteerWrist);
elevator.setPosition(wantedPuppeteerElevator);
} else if(!DriverStation.isTest()) {
currentState.update();
arm.setPosition(currentState.armPos);
if (superstructure.currentState != States.HOMING) {
Expand All @@ -303,7 +332,7 @@ public void update() {
} else {
var wristPos = edu.wpi.first.math.MathUtil.inputModulus(-wantedShooterPosition - arm.getPivotDegrees(), -0.5, 0.5);
wrist.setWristPosition(wristPos);
wantedAngle += a;
wantedAngle += shotWristdelta;

Logger.recordOutput("Wrist/Wanted Position Ground Relative", -wantedShooterPosition - arm.getPivotDegrees());
}
Expand All @@ -312,6 +341,7 @@ public void update() {
arm.stop();
elevator.stop();
}
prevState = currentState;
}

public void setWantedShooterPosition(double wantedPos) {
Expand Down
2 changes: 2 additions & 0 deletions src/main/java/org/codeorange/frc2024/subsystem/arm/Arm.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package org.codeorange.frc2024.subsystem.arm;

import edu.wpi.first.math.MathUtil;
import org.codeorange.frc2024.subsystem.AbstractSubsystem;
import org.littletonrobotics.junction.Logger;

Expand All @@ -18,6 +19,7 @@ public Arm(ArmIO armIO) {
* @param position The position to set the Arm (degrees)
*/
public void setPosition(double position) {
position = MathUtil.clamp(position, -0.02, 0.24);
armIO.setLeadPosition(position, 0);
Logger.recordOutput("Pivot/Goal position", position);
}
Expand Down
12 changes: 9 additions & 3 deletions src/main/java/org/codeorange/frc2024/subsystem/climber/Climber.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ public class Climber extends AbstractSubsystem {
public boolean climbing = false;
public boolean hasClimbed = false;
private boolean negated = false;
private double holdPosition;

public Climber(ClimberIO climberIO){
super();
Expand Down Expand Up @@ -51,18 +52,23 @@ public void update() {
}
}

if(climbing) {
if(climbing && !hasClimbed) {
if(!negated) {
climberIO.setMotorPosition(1);
climberIO.setVoltage(-8);
if (limitSwitchPushed()) {
climbing = false;
hasClimbed = true;
holdPosition = climberInputs.climberPosition + CLIMBER_SWITCH_OFFSET;
}
} else {
climberIO.setVoltage(4);
}
} else if(hasClimbed) {
climberIO.setMotorPosition(1);
climberIO.setMotorPosition(holdPosition);
}

if(limitSwitchPushed()) {
climberIO.stop();
}
}

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/org/codeorange/frc2024/subsystem/climber/ClimberIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,8 +71,8 @@ public ClimberIOTalonFX() {
climberCurrent = motor.getSupplyCurrent();
climberTemp = motor.getDeviceTemp();


BaseStatusSignal.setUpdateFrequencyForAll(50, climberPosition, climberVelocity, climberVoltage);
BaseStatusSignal.setUpdateFrequencyForAll(100, climberPosition);
BaseStatusSignal.setUpdateFrequencyForAll(50, climberVelocity, climberVoltage);
BaseStatusSignal.setUpdateFrequencyForAll(2.0, climberCurrent, climberTemp);

motor.optimizeBusUtilization();
Expand Down
Loading

0 comments on commit 9be41db

Please sign in to comment.