Skip to content

Commit 5344f68

Browse files
AntiBlaze1AntiBlaze1
committed
thursdays's arm workout
1 parent adc5f6d commit 5344f68

5 files changed

Lines changed: 115 additions & 41 deletions

File tree

‎src/main/java/frc/robot/subsystems/arm/Arm.java‎

Lines changed: 29 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -1,40 +1,38 @@
11
package frc.robot.subsystems.arm;
22

3+
import static frc.robot.subsystems.arm.ArmConstants.MAX_POSITION;
4+
import static frc.robot.subsystems.arm.ArmConstants.MIN_POSITION;
5+
36
import edu.wpi.first.math.MathUtil;
47
import edu.wpi.first.wpilibj2.command.Command;
58
import edu.wpi.first.wpilibj2.command.SubsystemBase;
6-
import org.littletonrobotics.junction.Logger;
7-
89
import java.util.function.DoubleSupplier;
9-
10-
import static frc.robot.subsystems.arm.ArmConstants.MAX_POSITION;
11-
import static frc.robot.subsystems.arm.ArmConstants.MIN_POSITION;
10+
import org.littletonrobotics.junction.Logger;
1211

1312
public class Arm extends SubsystemBase {
14-
private final ArmIO io;
15-
private final ArmIOInputsAutoLogged inputs=new ArmIOInputsAutoLogged();
16-
17-
public Arm(ArmIO io) {
18-
this.io = io;
19-
}
20-
21-
//bro why is default indentation for this 2 spaces
22-
23-
@Override
24-
public void periodic() {
25-
io.updateInputs(inputs);
26-
Logger.processInputs("Arm",inputs);
27-
}
28-
29-
public Command runPosition(DoubleSupplier angle) {
30-
return runEnd(
31-
()->io.setPosition(MathUtil.clamp(angle.getAsDouble(),MIN_POSITION,MAX_POSITION)),
32-
()->io.setPosition(0)
33-
).withName("Moving my arm");
34-
}
35-
36-
public Command stop() {
37-
return runOnce(()->io.setPosition(0))
38-
.withName("OH NO I STOPPED MOVING MY ARM");
39-
}
13+
private final ArmIO io;
14+
private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged();
15+
16+
public Arm(ArmIO io) {
17+
this.io = io;
18+
}
19+
20+
// bro why is default indentation for this 2 spaces
21+
22+
@Override
23+
public void periodic() {
24+
io.updateInputs(inputs);
25+
Logger.processInputs("Arm", inputs);
26+
}
27+
28+
public Command runPosition(DoubleSupplier angle) {
29+
return runEnd(
30+
() -> io.setPosition(MathUtil.clamp(angle.getAsDouble(), MIN_POSITION, MAX_POSITION)),
31+
() -> io.setPosition(0))
32+
.withName("Moving my arm");
33+
}
34+
35+
public Command stop() {
36+
return runOnce(() -> io.setPosition(0)).withName("OH NO I STOPPED MOVING MY ARM");
37+
}
4038
}

‎src/main/java/frc/robot/subsystems/arm/ArmConstants.java‎

Lines changed: 21 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,26 @@
11
package frc.robot.subsystems.arm;
22

3+
import edu.wpi.first.math.util.Units;
4+
35
public class ArmConstants {
4-
public static final double MAX_POSITION=300;
5-
public static final double MIN_POSITION=20;
6+
public static final double MAX_POSITION = 300;
7+
public static final double MIN_POSITION = 20;
8+
9+
public static final double MAX_VELOCITY = 4;
10+
public static final double MAX_ACCELERATION = 8;
11+
12+
// sim
13+
public static final double[] kPivotSimPID = {15, 0, 0, 0};
14+
public static final double[] kPivotSimFF = {0, 0.574, 0, 0};
15+
16+
// The P gain for the PID controller that drives this arm.
17+
public static final double kDefaultArmSetpointDegrees = Units.degreesToRadians(75.0);
18+
19+
// distance per pulse = (angle per revolution) / (pulses per revolution)
20+
// = (2 * PI rads) / (4096 pulses)
21+
public static final double kArmEncoderDistPerPulse = 1 / 4096;
622

7-
public static final double MAX_VELOCITY=4;
8-
public static final double MAX_ACCELERATION=8;
23+
public static final double kArmReduction = 200;
24+
public static final double kArmMass = 10.0; // Kilograms
25+
public static final double kArmLength = Units.inchesToMeters(20);
926
}

‎src/main/java/frc/robot/subsystems/arm/ArmIO.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,8 @@
55
public interface ArmIO {
66
@AutoLog
77
public class ArmIOInputs {
8-
// empty for now
8+
public double angleVelocityRadsPerSec = 0;
9+
public double angleRads = 0;
910
}
1011

1112
public default void updateInputs(ArmIOInputs inputs) {}

‎src/main/java/frc/robot/subsystems/arm/ArmIOSim.java‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
package frc.robot.subsystems.arm;
2+
3+
import edu.wpi.first.math.MathUtil;
4+
import edu.wpi.first.math.controller.ProfiledPIDController;
5+
import edu.wpi.first.math.system.plant.DCMotor;
6+
import edu.wpi.first.math.trajectory.TrapezoidProfile;
7+
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
8+
9+
public class ArmIOSim implements ArmIO {
10+
11+
private final ProfiledPIDController m_controller;
12+
private final DCMotor m_armGearbox = DCMotor.getNEO(1);
13+
private SingleJointedArmSim sim =
14+
new SingleJointedArmSim(
15+
m_armGearbox,
16+
ArmConstants.kArmReduction,
17+
SingleJointedArmSim.estimateMOI(ArmConstants.kArmLength, ArmConstants.kArmMass),
18+
ArmConstants.kArmLength,
19+
ArmConstants.MIN_POSITION,
20+
ArmConstants.MAX_POSITION,
21+
true,
22+
0.1);
23+
24+
public ArmIOSim() {
25+
m_controller =
26+
new ProfiledPIDController(
27+
ArmConstants.kPivotSimPID[0],
28+
ArmConstants.kPivotSimPID[1],
29+
ArmConstants.kPivotSimPID[2],
30+
new TrapezoidProfile.Constraints(2.45, 2.45));
31+
32+
m_controller.setTolerance(0.1, 0.05);
33+
}
34+
35+
@Override
36+
public void updateInputs(ArmIOInputs inputs) {
37+
sim.update(0.02);
38+
inputs.angleVelocityRadsPerSec = sim.getVelocityRadPerSec();
39+
inputs.angleVelocityRadsPerSec = sim.getAngleRads();
40+
}
41+
42+
@Override
43+
public void setVoltage(double voltage) {
44+
sim.setInputVoltage(voltage);
45+
}
46+
47+
@Override
48+
public void setPosition(double angleRads) {
49+
double pidOutput = m_controller.calculate(angleRads);
50+
sim.setInputVoltage(
51+
MathUtil.clamp(pidOutput, ArmConstants.MIN_POSITION, ArmConstants.MAX_POSITION));
52+
}
53+
54+
public void stop() {
55+
setPosition(0);
56+
}
57+
}

‎src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,13 @@
11
package frc.robot.subsystems.arm;
22

3-
import edu.wpi.first.math.controller.ProfiledPIDController;
4-
import edu.wpi.first.math.trajectory.TrapezoidProfile;
5-
63
import static frc.robot.subsystems.arm.ArmConstants.MAX_ACCELERATION;
74
import static frc.robot.subsystems.arm.ArmConstants.MAX_VELOCITY;
85

9-
public class ArmIOSparkMax implements ArmIO{
10-
private ProfiledPIDController controller=new ProfiledPIDController(0,0,0,new TrapezoidProfile.Constraints(MAX_VELOCITY,MAX_ACCELERATION));
6+
import edu.wpi.first.math.controller.ProfiledPIDController;
7+
import edu.wpi.first.math.trajectory.TrapezoidProfile;
118

9+
public class ArmIOSparkMax implements ArmIO {
10+
private ProfiledPIDController controller =
11+
new ProfiledPIDController(
12+
0, 0, 0, new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION));
1213
}

0 commit comments

Comments
 (0)