Skip to content

Commit

Permalink
improved arm code stubs - Aaron
Browse files Browse the repository at this point in the history
  • Loading branch information
@ProfessorAtomicManiac
ProfessorAtomicManiac committed Jan 31, 2024
1 parent aedeadd commit c5f4b75
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 33 deletions.
28 changes: 15 additions & 13 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,9 @@

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Units.*;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
Expand All @@ -20,19 +23,18 @@ public static final class Drivetrain {
}

public static final class Arm {
// Array Indexes (Just to make things easier to read)
public static final int ARM = 0;
//Motor port
public static final int MOTOR_PORT1 = 7;
public final static int MOTOR_PORT2 = 8;
public static final int LEFT_MOTOR_PORT = 7;
public final static int RIGHT_MOTOR_PORT = 8;
//all angles in rot here
public static final double intakeAngle = -.1;
public static final double ampAngle = .3;
public static final double speakerAngle = .4;
//TODO: finish understand why this is broken public static final Measure<Angle> INTAKE_ANGLE = Degrees.to(-1);
public static final double intakeAngle = Math.toDegrees(-0.1);
public static final double ampAngle = Math.toDegrees(0.3);
public static final double speakerAngle = Math.toDegrees(0.4);
//if needed
public static final double UPPER_ANGLE = Math.toRadians(70);
public static final double LOWER_ANGLE = Math.toRadians(0);
public static final double ARM_DICONT_RAD = (LOWER_ANGLE + UPPER_ANGLE) /2 - Math.PI;
public static final double UPPER_ANGLE_LIMIT = Math.toRadians(70);
public static final double LOWER_ANGLE_LIMIT = Math.toRadians(0);
public static final double ARM_DICONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI;
//Motor Controllers: pid, FF
public static final double[] pidVals = new double[] { 0.1, 0.0, 0.1 };
// Feedforward
Expand All @@ -42,9 +44,9 @@ public static final class Arm {
public static final double kA = 0.1;
// PID Constants
// placeholder numbers for now
public static double[] kP = {0.1, 0.1};
public static double[] kI = {0.1, 0.1};
public static double[] kD = {0.1, 0.1};
public static final double kP = 0.1;
public static final double kI = 0.1;
public static final double kD = 0.1;

//Arm measurements - ALL OF THEM ARE PLACEHOLDERS THE NUMBERS MEAN NOTHING
public static final double COM_ARM_LENGTH_METERS = 1;
Expand Down
30 changes: 10 additions & 20 deletions src/main/java/org/carlmontrobotics/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,8 @@


public class Arm extends SubsystemBase {
private final CANSparkMax armMotor1 = MotorControllerFactory.createSparkMax(Constants.Arm.MOTOR_PORT1,MotorConfig.NEO);
//private final CANSparkMax armMotor2 = MotorControllerFactory.createSparkMax(Constants.Arm.MOTOR_PORT2,MotorConfig.NEO);
private final CANSparkMax armMotor1 = MotorControllerFactory.createSparkMax(Constants.Arm.LEFT_MOTOR_PORT,MotorConfig.NEO);
//private final CANSparkMax armMotor2 = MotorControllerFactory.createSparkMax(Constants.Arm.RIGHT_MOTOR_PORT,MotorConfig.NEO);
//there is only one arm motor.
private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(Constants.Arm.kS, Constants.Arm.kV);
private final SparkAbsoluteEncoder armEncoder = armMotor1.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
Expand Down Expand Up @@ -78,35 +78,25 @@ public Translation2d getCoM() {
}

public void driveArm(TrapezoidProfile.State state) {

/*
ignore this math its wrong as it includes wrist
double kgv = getKg();
double armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(state.velocity, 0);
double armPIDVolts = armPID.calculate(getArmPos(), state.position);
if ((getArmPos() > Constants.Arm.UPPER_ANGLE && state.velocity > 0) ||
(getArmPos() < Constants.Arm.LOWER_ANGLE && state.velocity < 0)) {
if ((getArmPos() > Constants.Arm.UPPER_ANGLE_LIMIT && state.velocity > 0) ||
(getArmPos() < Constants.Arm.LOWER_ANGLE_LIMIT && state.velocity < 0)) {
armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0);
}
}
*/
public double getArmClampedGoal(double goal) {
//Find the limits of the arm. Used to move it and ensure that the arm does not move past the amount
return MathUtil.clamp(MathUtil.inputModulus(goal, Constants.Arm.ARM_DICONT_RAD, Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI), Constants.Arm.LOWER_ANGLE, Constants.Arm.UPPER_ANGLE);
return MathUtil.clamp(MathUtil.inputModulus(goal, Constants.Arm.ARM_DICONT_RAD, Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI), Constants.Arm.LOWER_ANGLE_LIMIT, Constants.Arm.UPPER_ANGLE_LIMIT);
}

@Override
public void periodic() {

//run driveArm constantly and pass in
}

public void driveArm(TrapezoidProfile.State state){
// set voltage to armMotor
double kgv = getKg();
double armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(state.velocity, 0);
double armPIDVolts = armPID.calculate(getArmPos(), state.position);
if ((getArmPos() > ARM_UPPER_LIMIT_RAD && state.velocity > 0) ||
(getArmPos() < ARM_LOWER_LIMIT_RAD && state.velocity < 0)) {
forbFlag = true;
armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0);
}
double volts = armFeedVolts + armPIDVolts;
armMotor.setVoltage(volts);
}
}

0 comments on commit c5f4b75

Please sign in to comment.