Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added a few manual controls for arm and elevator

Browse Source
This commit is contained in:
NoahLacks63 2025-01-15 02:04:25 +00:00
parent 865ba29152
commit afba8731b3
5 changed files with 99 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/frc/robot/RobotContainer.java
View File

@ -78,7 +78,7 @@ public class RobotContainer {
);
elevator.setDefaultCommand(
elevator.goToSetpoint(0, 1)
elevator.runElevator(operator::getLeftY)
);
indexer.setDefaultCommand(

2
src/main/java/frc/robot/constants/ArmConstants.java
View File

@ -5,4 +5,6 @@ public class ArmConstants {
public static final int kCANcoderID = 0;
public static final double kEncoderConversionFactor = 0;
public static final double kArmMaxVelocity = 0;
}

17
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -11,20 +11,29 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
public class ElevatorConstants {
public static final int kElevatorMotorID = 0;
public static final int kElevatorMotor1ID = 0;
public static final int kElevatorMotor2ID = 0;
public static final int kTopLimitSwitchID = 0;
public static final int kBottomLimitSwitchID = 0;
public static final double kEncoderConversionFactor = 0;
public static final int kMotorAmpsMax = 0;
public static final double kPIDControllerP = 0;
public static final double kPIDControllerI = 0;
public static final double kPIDControllerD = 0;
public static final double kPositionControllerP = 0;
public static final double kPositionControllerI = 0;
public static final double kPositionControllerD = 0;
public static final double kVelocityControllerP = 0;
public static final double kVelocityControllerI = 0;
public static final double kVelocityControllerD = 0;
public static final double kFeedForwardS = 0;
public static final double kFeedForwardG = 0;
public static final double kFeedForwardV = 0;
public static final double kElevatorMaxVelocity = 0;
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = 1;

31
src/main/java/frc/robot/subsystems/Arm.java
View File

@ -1,5 +1,7 @@
package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
@ -15,7 +17,9 @@ public class Arm extends SubsystemBase {
private CANcoder canCoder;
private PIDController pidController;
private PIDController positionController;
private PIDController velocityController;
private ArmFeedforward feedForward;
public Arm() {
@ -27,9 +31,26 @@ public class Arm extends SubsystemBase {
canCoder = new CANcoder(ArmConstants.kCANcoderID);
}
//manual command that keeps ouput speed consistent no matter the direction
public Command runArm(DoubleSupplier speed) {
return run(() -> {
double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
double voltsOut = velocityController.calculate(
rotationsToRadians(canCoder.getVelocity().getValueAsDouble()),
realSpeedTarget
) + feedForward.calculate(
rotationsToRadians(canCoder.getPosition().getValueAsDouble()),
canCoder.getVelocity().getValueAsDouble()
);
armMotor.setVoltage(voltsOut);
});
}
public Command goToSetpoint(double setpoint, double timeout) {
return run(() -> {
double voltsOut = pidController.calculate(
double voltsOut = positionController.calculate(
canCoder.getPosition().getValueAsDouble(),
setpoint
) + feedForward.calculate(
@ -38,6 +59,10 @@ public class Arm extends SubsystemBase {
);
armMotor.setVoltage(voltsOut);
}).until(pidController::atSetpoint).withTimeout(timeout);
}).until(positionController::atSetpoint).withTimeout(timeout);
}
protected double rotationsToRadians(double rotations) {
return rotations * 2 * Math.PI;
}
}

66
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -1,5 +1,7 @@
package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode;
@ -8,22 +10,33 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ElevatorConstants;
public class Elevator extends SubsystemBase {
protected SparkMax elevatorMotor1;
protected SparkMax elevatorMotor2;
protected RelativeEncoder encoder;
private PIDController pidController;
private DigitalInput topLimitSwitch;
private DigitalInput bottomLimitSwitch;
private PIDController positionController;
private PIDController velocityController;
private ElevatorFeedforward feedForward;
public Elevator() {
elevatorMotor1 = new SparkMax(
ElevatorConstants.kElevatorMotorID,
ElevatorConstants.kElevatorMotor1ID,
MotorType.kBrushless
);
elevatorMotor2 = new SparkMax(
ElevatorConstants.kElevatorMotor2ID,
MotorType.kBrushless
);
@ -33,13 +46,32 @@ public class Elevator extends SubsystemBase {
PersistMode.kPersistParameters
);
elevatorMotor2.configure(
ElevatorConstants.motorConfig.follow(ElevatorConstants.kElevatorMotor1ID),
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
encoder = elevatorMotor1.getEncoder();
pidController = new PIDController(
ElevatorConstants.kPIDControllerP,
ElevatorConstants.kPIDControllerI,
ElevatorConstants.kPIDControllerD
topLimitSwitch = new DigitalInput(
ElevatorConstants.kTopLimitSwitchID
);
bottomLimitSwitch = new DigitalInput(
ElevatorConstants.kBottomLimitSwitchID
);
positionController = new PIDController(
ElevatorConstants.kPositionControllerP,
ElevatorConstants.kPositionControllerI,
ElevatorConstants.kPositionControllerD
);
velocityController = new PIDController(
ElevatorConstants.kVelocityControllerP,
ElevatorConstants.kVelocityControllerI,
ElevatorConstants.kVelocityControllerD
);
feedForward = new ElevatorFeedforward(
@ -49,20 +81,32 @@ public class Elevator extends SubsystemBase {
);
}
public Command runElevator(double speed) {
//manual command that keeps ouput speed consistent no matter the direction
public Command runElevator(DoubleSupplier speed) {
return run(() -> {
elevatorMotor1.set(speed);
});
double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
double voltsOut = velocityController.calculate(
encoder.getVelocity(),
realSpeedTarget
) + feedForward.calculate(realSpeedTarget);
elevatorMotor1.setVoltage(voltsOut);
}).until(() -> topLimitSwitch.get() || bottomLimitSwitch.get());
}
//go to setpoint command
public Command goToSetpoint(double setpoint, double timeout) {
return run(() -> {
double voltsOut = pidController.calculate(
double voltsOut = positionController.calculate(
encoder.getPosition(),
setpoint
) + feedForward.calculate(0);
elevatorMotor1.setVoltage(voltsOut);
}).until(pidController::atSetpoint).withTimeout(timeout);
}).until(
() -> positionController.atSetpoint() || topLimitSwitch.get() || bottomLimitSwitch.get()
).withTimeout(timeout);
}
}