Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/climbing' into elevator
Browse files Browse the repository at this point in the history
  • Loading branch information
Brandon-Harad committed Jan 26, 2024
2 parents 69508b0 + af7557c commit f6555ad
Show file tree
Hide file tree
Showing 9 changed files with 220 additions and 0 deletions.
40 changes: 40 additions & 0 deletions src/main/java/org/team1540/robot2024/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,12 @@ public static class Drivetrain {
}

public static class Elevator {
public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25);
public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(48.0);
public static final double ELEVATOR_MINIMUM_HEIGHT = Units.inchesToMeters(27.0);
public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0);
public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + ELEVATOR_MAX_HEIGHT - ELEVATOR_MINIMUM_HEIGHT;

public static int talonId1;
public static int talonId2;
public static double kS = 0.25;
Expand All @@ -68,5 +74,39 @@ public static class Elevator {
public static int motionMagicCruiseVelocity = 80;
public static int motionMagicAcceleration = 160;
public static int motionMagicJerk = 1600;

public enum ElevatorState {
/**
* At max height :D
*/
TOP(ELEVATOR_MAX_HEIGHT),
/**
* At minimum height :D
*/
BOTTOM(ELEVATOR_MINIMUM_HEIGHT),
/**
* At height for top of initial climb :D
*/
CLIMB(254.0), //TODO: Find these values :D
/**
* At height for trap doing :D
*/
TRAP(254.0), //TODO: Find these values :D
/**
* At height for top of initial climb :D
*/
AMP(254.0); //TODO: Find these values :D

public final double heightMeters;
ElevatorState(double heightMeters) {
this.heightMeters = heightMeters;
}
}
}

public static class Tramp {
public static final double AMP_PERCENTAGE = 0.1540; //TODO: Find these values :D
public static final double TRAP_PERCENTAGE = 0.1678; //TODO: Find these values :D
public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
package org.team1540.robot2024.commands;

import edu.wpi.first.wpilibj2.command.Command;
import org.team1540.robot2024.Constants.Elevator.ElevatorState;
import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;

public class ElevatorSetpointCommand extends Command {
private final Elevator elevator;
private final ElevatorState state;
public ElevatorSetpointCommand(Elevator elevator, ElevatorState state) {
this.elevator = elevator;
this.state = state;
addRequirements(elevator);
}
@Override
public void initialize() {
elevator.goToSetpoint(state.heightMeters);
}

@Override
public void end(boolean interrupted) {
elevator.stop();
}
@Override
public boolean isFinished() {
return elevator.isAtSetpoint();
}

}
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
package org.team1540.robot2024.commands.climb;

import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import org.team1540.robot2024.Constants.Elevator.ElevatorState;
import org.team1540.robot2024.commands.ElevatorSetpointCommand;
import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;

public class ClimbSequence extends SequentialCommandGroup {
public ClimbSequence(Elevator elevator, Hooks hooks) { //TODO: Write servos no idea how they are supposed to work for now, add them somewhere :D
addCommands(
new ParallelCommandGroup(
new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)
//TODO: Put whatever drive/alignment command we plan on using here
),
new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB),
//TODO: Put whatever drive/alignment command we plan on using here
new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
hooks.deployHooksCommand() //TODO: Deploy hooks
);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
package org.team1540.robot2024.commands.climb;

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import org.team1540.robot2024.Constants.Elevator.ElevatorState;
import org.team1540.robot2024.commands.ElevatorSetpointCommand;
import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;

public class DeclimbSequence extends SequentialCommandGroup {
public DeclimbSequence(Elevator elevator, Hooks hooks) {
addCommands(
new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
hooks.undeployHooksCommand(), //Release hooks
new ElevatorSetpointCommand(elevator, ElevatorState.TOP)
);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
package org.team1540.robot2024.commands.climb;

import edu.wpi.first.wpilibj2.command.Command;
import org.team1540.robot2024.subsystems.fakesubsystems.Tramp;

//TODO: Write this command Tramp people :D
public class ScoreInTrap extends Command {
private final Tramp tramp;
public ScoreInTrap(Tramp tramp) {
this.tramp = tramp;
addRequirements(tramp);
}
@Override
public void initialize() {
//TODO: Score in trap :D
}

@Override
public void end(boolean interrupted) {

}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
package org.team1540.robot2024.commands.climb;

import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import org.team1540.robot2024.Constants;
import org.team1540.robot2024.Constants.Elevator.ElevatorState;
import org.team1540.robot2024.commands.ElevatorSetpointCommand;
import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
import org.team1540.robot2024.subsystems.fakesubsystems.Tramp;

public class TrapAndClimbSequence extends SequentialCommandGroup {

public TrapAndClimbSequence(Elevator elevator, Hooks hooks, Tramp tramp) {
addCommands(
new ClimbSequence(elevator, hooks), //Climb
new WaitCommand(0.1), //TODO: Perhaps remove this or change it depending on how climbing turns out to be
new ElevatorSetpointCommand(elevator, ElevatorState.TRAP),
new ParallelDeadlineGroup(
new WaitCommand(Constants.Tramp.TRAP_SCORING_TIME_SECONDS),
new ScoreInTrap(tramp) //TODO: Do whatever to this but not my job
),
new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)
);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
package org.team1540.robot2024.subsystems.fakesubsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;

//TODO: Get rid of this class and make an actual one its just a temporary thingie for David :D
public class Elevator extends SubsystemBase {
public void goToSetpoint(double heightMeters) {

}

public void stop() {

}

public boolean isAtSetpoint() {
return true;
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
package org.team1540.robot2024.subsystems.fakesubsystems;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
//TODO: Get rid of this class and make an actual one its just a temporary thingie for David :D
public class Hooks extends SubsystemBase {

//TODO: Whoever does this it should retract or deploy the hooks depending on if they are already deployed or retracted
public void deployHooks() {

}

public void undeployHooks() {

}

/**
* Factory method for deploying hooks
*/
public Command deployHooksCommand() {
return new InstantCommand(this::deployHooks, this);
}
/**
* Factory method for undeploying hooks
*/
public Command undeployHooksCommand() {
return new InstantCommand(this::undeployHooks, this);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
package org.team1540.robot2024.subsystems.fakesubsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
//TODO: Get rid of this class and make an actual one its just a temporary thingie for David :D

public class Tramp extends SubsystemBase {
public void setPercent(double percentage) {

}
public boolean hasScored() {
return true;
}
}

0 comments on commit f6555ad

Please sign in to comment.