Skip to content

Commit

Permalink
Merge pull request #57 from IronRiders/elevator-testing
Browse files Browse the repository at this point in the history 
Elevator system update!
  • Loading branch information
@a-mishmash
a-mishmash authored Feb 21, 2025
2 parents 3e9d956 + 1ac0c85 commit 9e46d4c
Show file tree
Hide file tree
Showing 7 changed files with 193 additions and 191 deletions.
2 changes: 1 addition & 1 deletion src/main/java/org/ironriders/coral/CoralWristCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ public CoralWristCommands(CoralWristSubsystem wrist) {

public Command set(State state) {
return coralWrist
.runOnce(() -> coralWrist.set(state.getPostion()))
.runOnce(() -> coralWrist.setGoal(state.getPostion()))
.until(coralWrist::atPosition)
.handleInterrupt(coralWrist::reset);
}
Expand Down
7 changes: 3 additions & 4 deletions src/main/java/org/ironriders/coral/CoralWristConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ public class CoralWristConstants {
public static final int CORALWRISTENCODER = -1;

// Need to tune
public static final double CORALWRISTKP = 0.55;
public static final double CORALWRISTKP = 0.5;
public static final double CORALWRISTKI = 0;
public static final double CORALWRISTKD = 0.0;
// public static final double CORALWRISTKS = 0.0; //The static gain in volts. //
Expand All @@ -21,9 +21,8 @@ public class CoralWristConstants {
// public static final double CORALWRISTKV = 0.0; // The velocity gain in
// V/(rad/s).

public static final int CORAL_WRIST_CURRENT_STALL_LIMIT = 30; // please test
// public static final int CORAL_WRIST_COMPENSATED_VOLTAGE = 10; // ASK A MENTOR PLEASE
public static final double CORAL_WRIST_ENCODER_OFFSET = -1; // please test
public static final int CORAL_WRIST_CURRENT_STALL_LIMIT = 1; // please test
public static final double CORAL_WRIST_ENCODER_OFFSET = 0; // please test
public static final double CORAL_WRIST_TOLERANCE = 10; // tune me please

public enum State {
Expand Down
38 changes: 25 additions & 13 deletions src/main/java/org/ironriders/coral/CoralWristSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package org.ironriders.coral;

import org.ironriders.core.Utils;
import org.ironriders.elevator.ElevatorConstants;

import com.revrobotics.spark.SparkLimitSwitch;
import com.revrobotics.spark.SparkMax;
Expand All @@ -11,11 +12,15 @@
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkBase.ResetMode;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static org.ironriders.coral.CoralWristConstants.*;
import static org.ironriders.elevator.ElevatorConstants.D;
import static org.ironriders.elevator.ElevatorConstants.I;
import static org.ironriders.elevator.ElevatorConstants.P;

public class CoralWristSubsystem extends SubsystemBase {
// Why do we extend subsystem base?
Expand All @@ -24,35 +29,40 @@ public class CoralWristSubsystem extends SubsystemBase {

// find acutal motor IDs
private final SparkMax motor = new SparkMax(CORALWRISTMOTOR, MotorType.kBrushless);
private final ProfiledPIDController pid = new ProfiledPIDController(0.1, 0, 0, PROFILE);
private final PIDController pid = new PIDController(P, I, D);
private final DutyCycleEncoder absoluteEncoder = new DutyCycleEncoder(CORALWRISTENCODER);
private final SparkLimitSwitch forwardLimitSwitch = motor.getForwardLimitSwitch();
private final SparkLimitSwitch reverseLimitSwitch = motor.getReverseLimitSwitch();
private final LimitSwitchConfig forwardLimitSwitchConfig = new LimitSwitchConfig();
private final LimitSwitchConfig reverseLimitSwitchConfig = new LimitSwitchConfig();
private final SparkMaxConfig motorConfig = new SparkMaxConfig();
private TrapezoidProfile.State goalState = new TrapezoidProfile.State();
private TrapezoidProfile.State setPointState = new TrapezoidProfile.State();
private final TrapezoidProfile profile;

// private ArmFeedforward coralFeedforward = new
// ArmFeedforward(CORALWRISTKS,CORALWRISTKG,CORALWRISTKV);
public CoralWristSubsystem() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ElevatorConstants.MAX_VEL, ElevatorConstants.MAX_ACC);
profile = new TrapezoidProfile(constraints);

forwardLimitSwitchConfig.forwardLimitSwitchEnabled(true)
.forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyClosed); // this sets allows the limit switch to
// disable the motor
forwardLimitSwitchConfig.forwardLimitSwitchEnabled(true)
.forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyClosed); // It also sets the Type to k normally
reverseLimitSwitchConfig.reverseLimitSwitchEnabled(true)
.reverseLimitSwitchType(LimitSwitchConfig.Type.kNormallyClosed); // It also sets the Type to k normally
// closed see
// https://docs.revrobotics.com/brushless/spark-max/specs/data-port#limit-switch-operation
motorConfig
.smartCurrentLimit(CORAL_WRIST_CURRENT_STALL_LIMIT)
// .voltageCompensation(CORAL_WRIST_COMPENSATED_VOLTAGE)
.idleMode(IdleMode.kBrake).limitSwitch
.idleMode(IdleMode.kBrake)
.apply(forwardLimitSwitchConfig)
.apply(reverseLimitSwitchConfig);

motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

set(getRotation());
setGoal(getRotation());

pid.setTolerance(CORAL_WRIST_TOLERANCE);

Expand All @@ -61,31 +71,33 @@ public CoralWristSubsystem() {

@Override
public void periodic() {
double output = pid.calculate(getRotation());
setPointState = profile.calculate(ElevatorConstants.T, setPointState, goalState);
SmartDashboard.putNumber("Coral Wrist Set Postion", setPointState.position);
double output = pid.calculate(getRotation(),setPointState.position);
motor.set(output);

SmartDashboard.putNumber(DASHBOARD_PREFIX + "rotation", getRotation());
SmartDashboard.putNumber(DASHBOARD_PREFIX + "output", output);
SmartDashboard.putNumber(DASHBOARD_PREFIX + "setPoint", pid.getGoal().position);
SmartDashboard.putNumber(DASHBOARD_PREFIX + "setPoint", goalState.position);
SmartDashboard.putBoolean(DASHBOARD_PREFIX + "fowardSwitch", forwardLimitSwitch.isPressed());
SmartDashboard.putBoolean(DASHBOARD_PREFIX + "reverseSwitch", reverseLimitSwitch.isPressed());
}

public void set(double position) {
pid.setGoal(position);
public void setGoal(double position) {
goalState = new TrapezoidProfile.State(position, 0);
}

public void reset() {
pid.setGoal(getRotation()); // Stops the wrist from moving
pid.reset(getRotation()); // sets the error to zero but asssums it has no velocity
goalState = new TrapezoidProfile.State(0, 0);

}

private double getRotation() {
return Utils.absoluteRotation(absoluteEncoder.get() * 360 - CORAL_WRIST_ENCODER_OFFSET);
}

public boolean atPosition() {
return pid.atGoal();
return pid.atSetpoint();
}

public CoralWristCommands getCommands() {
Expand Down
170 changes: 85 additions & 85 deletions src/main/java/org/ironriders/core/RobotCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,44 +46,44 @@ public class RobotCommands {

private final DriveCommands driveCommands;
private final ElevatorCommands elevatorCommands;
private final CoralWristCommands coralWristCommands;
private final CoralIntakeCommands coralIntakeCommands;
private final AlgaeWristCommands algaeWristCommands;
private final AlgaeIntakeCommands algaeIntakeCommands;
// private final CoralWristCommands coralWristCommands;
// private final CoralIntakeCommands coralIntakeCommands;
// private final AlgaeWristCommands algaeWristCommands;
// private final AlgaeIntakeCommands algaeIntakeCommands;
private final VisionCommands visionCommands;
private final GenericHID controller;

public RobotCommands(
DriveCommands driveCommands,
ElevatorCommands elevatorCommands,
CoralWristCommands coralWristCommands, CoralIntakeCommands coralIntakeCommands,
AlgaeWristCommands algaeWristCommands, AlgaeIntakeCommands algaeIntakeCommands,
// CoralWristCommands coralWristCommands, CoralIntakeCommands coralIntakeCommands,
// AlgaeWristCommands algaeWristCommands, AlgaeIntakeCommands algaeIntakeCommands,
VisionCommands visionCommands, GenericHID controller) {
this.driveCommands = driveCommands;
this.elevatorCommands = elevatorCommands;
this.coralWristCommands = coralWristCommands;
this.coralIntakeCommands = coralIntakeCommands;
this.algaeWristCommands = algaeWristCommands;
this.algaeIntakeCommands = algaeIntakeCommands;
// this.coralWristCommands = coralWristCommands;
// this.coralIntakeCommands = coralIntakeCommands;
// this.algaeWristCommands = algaeWristCommands;
// this.algaeIntakeCommands = algaeIntakeCommands;
this.visionCommands = visionCommands;
this.controller = controller;

// register named commands
NamedCommands.registerCommand("Prepare to Score Algae", this.prepareToScoreAlgae());
NamedCommands.registerCommand("Score Algae", this.scoreAlgae());
// NamedCommands.registerCommand("Prepare to Score Algae", this.prepareToScoreAlgae());
// NamedCommands.registerCommand("Score Algae", this.scoreAlgae());

NamedCommands.registerCommand("Prepare to Score Coral L1", this.prepareToScoreCoral(ElevatorConstants.Level.L1));
NamedCommands.registerCommand("Prepare to Score Coral L2", this.prepareToScoreCoral(ElevatorConstants.Level.L2));
NamedCommands.registerCommand("Prepare to Score Coral L3", this.prepareToScoreCoral(ElevatorConstants.Level.L3));
NamedCommands.registerCommand("Prepare to Score Coral L4", this.prepareToScoreCoral(ElevatorConstants.Level.L4));
NamedCommands.registerCommand("Score Coral", this.scoreCoral());
// NamedCommands.registerCommand("Prepare to Score Coral L1", this.prepareToScoreCoral(ElevatorConstants.Level.L1));
// NamedCommands.registerCommand("Prepare to Score Coral L2", this.prepareToScoreCoral(ElevatorConstants.Level.L2));
// NamedCommands.registerCommand("Prepare to Score Coral L3", this.prepareToScoreCoral(ElevatorConstants.Level.L3));
// NamedCommands.registerCommand("Prepare to Score Coral L4", this.prepareToScoreCoral(ElevatorConstants.Level.L4));
// NamedCommands.registerCommand("Score Coral", this.scoreCoral());

NamedCommands.registerCommand("Prepare to Grab Low Algae", this.prepareToGrabAlgae(ElevatorConstants.Level.L3));
NamedCommands.registerCommand("Prepare to Grab High Algae", this.prepareToGrabAlgae(ElevatorConstants.Level.L4));
NamedCommands.registerCommand("Grab Algae", this.grabAlgae());
// NamedCommands.registerCommand("Prepare to Grab Low Algae", this.prepareToGrabAlgae(ElevatorConstants.Level.L3));
// NamedCommands.registerCommand("Prepare to Grab High Algae", this.prepareToGrabAlgae(ElevatorConstants.Level.L4));
// NamedCommands.registerCommand("Grab Algae", this.grabAlgae());

NamedCommands.registerCommand("Prepare to Grab Coral", this.prepareToGrabCoral());
NamedCommands.registerCommand("Grab Coral", this.grabCoral());
// NamedCommands.registerCommand("Prepare to Grab Coral", this.prepareToGrabCoral());
// NamedCommands.registerCommand("Grab Coral", this.grabCoral());
}

/**
Expand Down Expand Up @@ -119,67 +119,67 @@ public Command toggleClimber() {
// TODO
}

public Command prepareToScoreAlgae() {
return Commands.parallel(
elevatorCommands.set(ElevatorConstants.Level.Down),
algaeWristCommands.set(AlgaeWristState.EXTENDED));
}

public Command scoreAlgae() {
// TODO: option to grab coral in parallel
return Commands.sequence(
algaeIntakeCommands.set(AlgaeIntakeState.EJECT),
algaeIntakeCommands.set(AlgaeIntakeState.STOP),
algaeWristCommands.set(AlgaeWristState.STOWED));
}

public Command prepareToScoreCoral(ElevatorConstants.Level level) {
CoralWristConstants.State wristState = switch (level) {
case L1, L2, L3 -> CoralWristConstants.State.L1toL3;
case L4 -> CoralWristConstants.State.L4;
default -> {
throw new IllegalArgumentException("Cannot score coral to level: " + level);
}
};

return Commands.parallel(
elevatorCommands.set(level),
coralWristCommands.set(wristState));
}

public Command scoreCoral() {
return Commands.sequence(
coralIntakeCommands.set(CoralIntakeConstants.State.EJECT),
Commands.parallel(
coralWristCommands.set(CoralWristConstants.State.STOWED),
elevatorCommands.set(ElevatorConstants.Level.Down)));
}

public Command prepareToGrabAlgae(ElevatorConstants.Level level) {
return Commands.parallel(
elevatorCommands.set(level),
algaeWristCommands.set(AlgaeWristState.EXTENDED),
algaeIntakeCommands.set(AlgaeIntakeState.GRAB));
}

public Command grabAlgae() {
return Commands.sequence(
algaeIntakeCommands.set(AlgaeIntakeState.GRAB),
algaeWristCommands.set(AlgaeWristState.STOWED),
this.rumble());
}

public Command prepareToGrabCoral() {
return Commands.parallel(
elevatorCommands.set(ElevatorConstants.Level.Down),
coralWristCommands.set(CoralWristConstants.State.STATION)
);
}

public Command grabCoral() {
return Commands.sequence(
coralIntakeCommands.set(CoralIntakeConstants.State.GRAB),
coralWristCommands.set(CoralWristConstants.State.STOWED),
rumble());
}
// public Command prepareToScoreAlgae() {
// return Commands.parallel(
// elevatorCommands.set(ElevatorConstants.Level.Down),
// algaeWristCommands.set(AlgaeWristState.EXTENDED));
// }

// public Command scoreAlgae() {
// // TODO: option to grab coral in parallel
// return Commands.sequence(
// algaeIntakeCommands.set(AlgaeIntakeState.EJECT),
// algaeIntakeCommands.set(AlgaeIntakeState.STOP),
// algaeWristCommands.set(AlgaeWristState.STOWED));
// }

// public Command prepareToScoreCoral(ElevatorConstants.Level level) {
// CoralWristConstants.State wristState = switch (level) {
// case L1, L2, L3 -> CoralWristConstants.State.L1toL3;
// case L4 -> CoralWristConstants.State.L4;
// default -> {
// throw new IllegalArgumentException("Cannot score coral to level: " + level);
// }
// };

// return Commands.parallel(
// elevatorCommands.set(level),
// coralWristCommands.set(wristState));
// }

// public Command scoreCoral() {
// return Commands.sequence(
// coralIntakeCommands.set(CoralIntakeConstants.State.EJECT),
// Commands.parallel(
// coralWristCommands.set(CoralWristConstants.State.STOWED),
// elevatorCommands.set(ElevatorConstants.Level.Down)));
// }

// public Command prepareToGrabAlgae(ElevatorConstants.Level level) {
// return Commands.parallel(
// elevatorCommands.set(level),
// algaeWristCommands.set(AlgaeWristState.EXTENDED),
// algaeIntakeCommands.set(AlgaeIntakeState.GRAB));
// }

// public Command grabAlgae() {
// return Commands.sequence(
// algaeIntakeCommands.set(AlgaeIntakeState.GRAB),
// algaeWristCommands.set(AlgaeWristState.STOWED),
// this.rumble());
// }

// public Command prepareToGrabCoral() {
// return Commands.parallel(
// elevatorCommands.set(ElevatorConstants.Level.Down),
// coralWristCommands.set(CoralWristConstants.State.STATION)
// );
// }

// public Command grabCoral() {
// return Commands.sequence(
// coralIntakeCommands.set(CoralIntakeConstants.State.GRAB),
// coralWristCommands.set(CoralWristConstants.State.STOWED),
// rumble());
// }
}
Loading

0 comments on commit 9e46d4c

Please sign in to comment.