Skip to content

Commit

Permalink
Revert "finished arm subsystem"
Browse files Browse the repository at this point in the history 
This reverts commit f141379.
  • Loading branch information
@ProfessorAtomicManiac
ProfessorAtomicManiac committed Mar 2, 2024
1 parent 65c7640 commit 52bca56
Show file tree
Hide file tree
Showing 4 changed files with 59 additions and 62 deletions.
14 changes: 7 additions & 7 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,13 +86,13 @@ public static final class Manipulator {
public static final int port = 1;

// Arm commands
public static final int RAISE_TO_SPEAKER_POD_BUTTON = Button.kY.value;
public static final int RAISE_TO_AMP_BUTTON = Button.kB.value;
public static final int RAISE_TO_SPEAKER_SAFE_BUTTON = Button.kA.value;
public static final int RAISE_TO_SPEAKER_NEXT_BUTTON = Button.kX.value;
public static final int RAISE_TO_GROUND_BUTTON = Button.kStart.value;
public static final int RAISE_TO_CLIMBER_BUTTON = Button.kLeftBumper.value;
public static final int LOWER_TO_CLIMBER_BUTTON = Button.kRightBumper.value;
public static final int raiseToSpeakerPodButton = Button.kY.value;
public static final int raiseToAmpButton = Button.kB.value;
public static final int raiseToSpeakerSafeButton = Button.kA.value;
public static final int raiseToSpeakerNextButton = Button.kX.value;
public static final int raiseToGroundButton = Button.kStart.value;
public static final int raiseToClimberButton = Button.kLeftBumper.value;
public static final int lowerToClimberButton = Button.kRightBumper.value;
}
}
}
15 changes: 9 additions & 6 deletions src/main/java/org/carlmontrobotics/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,8 @@
//199 files
// import org.carlmontrobotics.subsystems.*;
import org.carlmontrobotics.commands.*;
import static org.carlmontrobotics.Constants.OI.Manipulator.*;
import static org.carlmontrobotics.Constants.Arm.*;
import static org.carlmontrobotics.Constants.OI;
import static org.carlmontrobotics.Constants.Arm.ampAngle;

import org.carlmontrobotics.Constants.OI;
import org.carlmontrobotics.subsystems.Arm;
Expand Down Expand Up @@ -72,12 +72,15 @@ private void setBindingsManipulator() {
//right joystick used for manual arm control

// Speaker Buttons
new JoystickButton(manipulatorController, RAISE_TO_SPEAKER_POD_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(SPEAKER_ANGLE);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerPodButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle1);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerNextButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle2);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerSafeButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle3);}));
// Amp and Intake Buttons
new JoystickButton(manipulatorController, RAISE_TO_AMP_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(AMP_ANGLE);}));
new JoystickButton(manipulatorController, RAISE_TO_GROUND_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(INTAKE_ANGLE);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToAmpButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.ampAngle);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToGroundButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.intakeAngle);}));
// Cimber Buttons
new JoystickButton(manipulatorController, RAISE_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(CLIMB_ANGLE);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToClimberButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberUpAngle);}));
new JoystickButton(manipulatorController, Constants.Arm.lowerToClimberButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberDownAngle);}));
}

public Command getAutonomousCommand() {
Expand Down
37 changes: 20 additions & 17 deletions src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,18 +14,19 @@
import edu.wpi.first.wpilibj.XboxController;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj.DriverStation;

import edu.wpi.first.wpilibj.Timer;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.trajectory.TrapezoidProfile;


public class ArmTeleop extends Command {
private final DoubleSupplier joystick;
private final DoubleSupplier joystick;
private final Arm armSubsystem;
private final double kDt = 0.02;
private double lastTime = 0;
private Timer armTimer = new Timer();
TrapezoidProfile.State goalState;

/** Creates a new ArmTeleop. */
public ArmTeleop(Arm armSubsystem, DoubleSupplier joystickSupplier) {
// Use addRequirements() here to declare subsystem dependencies.
Expand All @@ -36,24 +37,24 @@ public ArmTeleop(Arm armSubsystem, DoubleSupplier joystickSupplier) {
// Called when the command is initially scheduled.
@Override
public void initialize() {
goalState = armSubsystem.getCurrentArmState();
goalState = new TrapezoidProfile.State(armSubsystem.getArmPos(), armSubsystem.getArmVel());
lastTime = Timer.getFPGATimestamp();
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
// use trapazoid math and controllerMoveArm method from arm subsytem to apply
// voltage to the motor
double speed = getRequestedSpeeds();

double goalArmRad = goalState.position + speed * kDt;
goalArmRad = MathUtil.clamp(goalArmRad, ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD);
goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD,
armSubsystem.getArmPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);

if ((speed != 0) && !DriverStation.isAutonomous()) {
armSubsystem.setArmTarget(goalArmRad);
}
// use trapazoid math and controllerMoveArm method from arm subsytem to apply voltage to the motor
double speeds = getRequestedSpeeds();
double currTime = Timer.getFPGATimestamp();
double deltaT = currTime - lastTime;

double goalArmRad = goalState.position + speeds * deltaT;
goalArmRad = MathUtil.clamp(goalArmRad, LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT);
goalState = new TrapezoidProfile.State(goalArmRad, 0);
TrapezoidProfile.State setpoint = armSubsystem.calculateCustomSetPoint(armTimer.get(), armSubsystem.getCurrentArmState(), goalState);
armSubsystem.driveArm(setpoint.position);
lastTime = currTime;
}

public double getRequestedSpeeds() {
Expand All @@ -70,6 +71,8 @@ public double getRequestedSpeeds() {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
armTimer.stop();
armTimer.reset();
}

// Returns true when the command should end.
Expand Down
55 changes: 23 additions & 32 deletions src/main/java/org/carlmontrobotics/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
package org.carlmontrobotics.subsystems;

import static org.carlmontrobotics.Constants.Arm.*;

import org.carlmontrobotics.commands.ArmTeleop;
Expand Down Expand Up @@ -30,21 +29,20 @@
// TODO: FIGURE OUT ANGLES
// Arm angle is measured from horizontal on the intake side of the robot and bounded between __ and __
public class Arm extends SubsystemBase {

private final CANSparkMax masterArmMotor = MotorControllerFactory.createSparkMax(MASTER_ARM_MOTOR, MotorConfig.NEO);
private final CANSparkMax followArmMotor = MotorControllerFactory.createSparkMax(FOLLOW_ARM_MOTOR, MotorConfig.NEO);
private final SparkAbsoluteEncoder armEncoder = masterArmMotor
.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
private final SparkAbsoluteEncoder armEncoder = masterArmMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);

private static double kDt = 0.02;

// PID, feedforward, trap profile
//PID, feedforward, trap profile
private final ArmFeedforward armFeed = new ArmFeedforward(kG, kS, kV, kA);
private final SparkPIDController armPID = masterArmMotor.getPIDController();

private TrapezoidProfile armProfile = new TrapezoidProfile(armConstraints);

// TODO: put in correct initial position
//TODO: put in correct initial position
// rad, rad/s
private static TrapezoidProfile.State goalState;
private static TrapezoidProfile.State setpointState;
Expand All @@ -58,7 +56,7 @@ public Arm() {
armEncoder.setVelocityConversionFactor(rotationToRad);
armEncoder.setInverted(encoderInverted);
followArmMotor.follow(masterArmMotor);

armEncoder.setZeroOffset(ENCODER_OFFSET);
armPID.setFeedbackDevice(armEncoder);
armPID.setPositionPIDWrappingEnabled(true);
Expand All @@ -68,7 +66,7 @@ public Arm() {
armPID.setP(kP);
armPID.setI(kI);
armPID.setD(kD);

SmartDashboard.putData("Arm", this);

setpointState = getCurrentArmState();
Expand All @@ -79,21 +77,19 @@ public Arm() {

@Override
public void periodic() {
if (DriverStation.isDisabled())
resetGoal();
if(DriverStation.isDisabled()) resetGoal();
driveArm();
autoCancelArmCommand();
}

public void autoCancelArmCommand() {
if (!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous())
return;
public void autoCancelArmCommand() {
if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return;

double requestedSpeeds = ((ArmTeleop) getDefaultCommand()).getRequestedSpeeds();

if (requestedSpeeds != 0) {
if(requestedSpeeds != 0) {
Command currentArmCommand = getCurrentCommand();
if (currentArmCommand != getDefaultCommand() && currentArmCommand != null) {
if(currentArmCommand != getDefaultCommand() && currentArmCommand != null) {
currentArmCommand.cancel();
}
}
Expand All @@ -103,23 +99,19 @@ public void autoCancelArmCommand() {
// uses trapezoid profiles which supplies goal states to pid controller
// feedforward controller is used to supply additional voltage to keep it
// at its current position
// YOU DO NOT HAVE TO WORRY ABOUT HOW THE ARM DRIVES OUTSIDE OF SUBSYSTEM
public void driveArm() {
public void driveArm(){
setpointState = armProfile.calculate(kDt, setpointState, goalState);
var currentPosition = getCurrentArmState();
double armFeedVolts = armFeed.calculate(currentPosition.position, currentPosition.velocity);

// code to stop arm from moving past certain bounds
if ((getArmPos() > ARM_UPPER_LIMIT_RAD && currentPosition.velocity > 0) ||
(getArmPos() < ARM_LOWER_LIMIT_RAD && currentPosition.velocity < 0)) {
if ((getArmPos() > ARM_UPPER_LIMIT_RAD && currentPosition.velocity > 0) ||
(getArmPos() < ARM_LOWER_LIMIT_RAD && currentPosition.velocity < 0)) {
armFeedVolts = armFeed.calculate(currentPosition.position, 0);
}
armPID.setReference(setpointState.position, CANSparkBase.ControlType.kPosition, 0, armFeedVolts);
}

// sets target arm position
// automatically clamps target positions such that they do not exceed lower and
// upper limit
public void setArmTarget(double targetPos) {
targetPos = getArmClampedGoal(targetPos);
goalState.position = targetPos;
Expand All @@ -131,9 +123,9 @@ public void resetGoal() {
setArmTarget(armPos);
}

// #endregion
//#endregion

// #region Getters
//#region Getters

public double getArmPos() {
return MathUtil.inputModulus(armEncoder.getPosition(), ARM_DISCONTINUITY_RAD,
Expand All @@ -143,22 +135,21 @@ public double getArmPos() {
public double getArmVel() {
return armEncoder.getVelocity();
}

public TrapezoidProfile.State getCurrentArmState() {
return new TrapezoidProfile.State(getArmPos(), getArmVel());
}

public TrapezoidProfile.State getCurrentArmGoal() {
return goalState;
}

public boolean armAtGoal() {
return Math.abs(getArmPos() - goalState.position) < posToleranceRad &&
Math.abs(getArmVel() - goalState.velocity) < velToleranceRadPSec;
Math.abs(getArmVel() - goalState.velocity) < velToleranceRadPSec;
}

public double getArmClampedGoal(double goal) {
return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONTINUITY_RAD, ARM_DISCONTINUITY_RAD + 2 * Math.PI),
ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD);
return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONTINUITY_RAD, ARM_DISCONTINUITY_RAD + 2 * Math.PI), ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD);
}
}

0 comments on commit 52bca56

Please sign in to comment.