Skip to content

Commit

Permalink
made it look nice :)
Browse files Browse the repository at this point in the history
  • Loading branch information
sofiebudman committed Mar 3, 2024
1 parent 69a70a9 commit 080ede3
Showing 1 changed file with 1 addition and 24 deletions.
25 changes: 1 addition & 24 deletions src/main/java/org/carlmontrobotics/subsystems/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -101,14 +101,10 @@ public Arm() {
armPIDMaster.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT);
//two PIDs?



SmartDashboard.putData("Arm", this);

setPoint = getCurrentArmState();

goalState = getCurrentArmState();

setArmTarget(goalState.position);
}

Expand All @@ -135,14 +131,10 @@ public void periodic() {

// SmartDashboard.putNumber("ArmPos", getArmPos());



autoCancelArmCommand();
driveArm();
}



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

Expand Down Expand Up @@ -176,8 +168,6 @@ public void setArmTarget(double targetPos) {
goalState.velocity = 0;
}



public void resetGoal() {
double armPos = getArmPos();
setArmTarget(armPos);
Expand Down Expand Up @@ -222,37 +212,24 @@ public double getArmPos() {
ARM_DISCONT_RAD + 2 * Math.PI);
}


public double getArmVel() {
return armMasterEncoder.getVelocity();
}


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


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


public boolean armAtSetpoint() {
return Math.abs(getArmPos() - goalState.position) < POS_TOLERANCE_RAD &&
Math.abs(getArmVel() - goalState.velocity) < VEL_TOLERANCE_RAD_P_SEC;
}



public double getArmClampedGoal(double goal) {
return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI), LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT);
}







}
}

0 comments on commit 080ede3

Please sign in to comment.