Skip to content

Commit

Permalink
pushed changes from 3/10
Browse files Browse the repository at this point in the history
  • Loading branch information
@ProfessorAtomicManiac
ProfessorAtomicManiac committed Mar 11, 2024
1 parent 7d6d875 commit d3778c1
Show file tree
Hide file tree
Showing 3 changed files with 39 additions and 24 deletions.
32 changes: 18 additions & 14 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,9 @@ public static final class Arm {
public static final boolean MOTOR_INVERTED_FOLLOWER = true; //verifyed by design AND physical testing

public static final double ROTATION_TO_RAD = 2 * Math.PI;
public static final boolean ENCODER_INVERTED = false;
public static final boolean ENCODER_INVERTED = true;

public static final int MAX_VOLTAGE = 12;
public static final double ENCODER_OFFSET_RAD = 0;
public static final double ENCODER_OFFSET_RAD = 2.678 - 0.6095;

// TODO: finish understand why this is broken public static final Measure<Angle>
// INTAKE_ANGLE = Degrees.to(-1);
Expand All @@ -56,16 +55,21 @@ public static final class Arm {
public static final double CLIMBER_DOWN_ANGLE_RAD = Units.degreesToRadians(24);

// PID, Feedforward, Trapezoid
public static final double kP = 0.1;
public static final double kI = 0.1;
public static final double kD = 0.1;
public static final double kS = 0.1;
public static final double kG = 0.1;
public static final double kV = Units.rotationsToRadians(0.1);
public static final double kA = Units.rotationsToRadians(0.1);
public static final double IZONE_RAD = .09;
public static final double kP = 50; //5.7938 / (2 * Math.PI);
public static final double kI = 0;
public static final double kD = 1.0761 / (2 * Math.PI);
public static final double kS = 0.1498;
public static final double kG = 0.3489;
public static final double kV = 5.7539 / (2 * Math.PI);
public static final double kA = 0.9569 / (2 * Math.PI);
public static final double IZONE_RAD = 0;
//fine for now, change it later before use - ("Incorect use of setIZone()" Issue #22)
public static final double MAX_FF_ACCEL_RAD_P_S = 53.728; // rad / s^2 ((.89*2)/(1.477/(61.875^2))/61.875)-20.84
public static final double MAX_FF_VEL_RAD_P_S = 0.2; //rad/s Aarav did the work
public static final double MAX_FF_ACCEL_RAD_P_S = .0183; // rad / s^2 Aarav did the math


public static final double MIN_VOLTAGE = -12; //-((kS + kG + 1)/12);
public static final double MAX_VOLTAGE = 12; //(kS + kG + 1)/12;

// if needed
public static final double COM_ARM_LENGTH_METERS = 0.381;
Expand All @@ -82,8 +86,8 @@ public static final class Arm {
// idk TODO: test on actual encoder without a conversion
// factor
public static final double VEL_TOLERANCE_RAD_P_SEC = (POS_TOLERANCE_RAD/0.02); // 20ms per robot loop
public static final double UPPER_ANGLE_LIMIT_RAD = Units.degreesToRadians(70);
public static final double LOWER_ANGLE_LIMIT_RAD = Units.degreesToRadians(0);
public static final double UPPER_ANGLE_LIMIT_RAD = 1.63;
public static final double LOWER_ANGLE_LIMIT_RAD = -0.5;
public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI;

public static final double DISCONNECTED_ENCODER_TIMEOUT_SEC = 0.25;
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/org/carlmontrobotics/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,8 @@ private void setDefaultCommands() {
// () -> driverController.getRawButton(OI.Driver.slowDriveButton)
// ));

arm.setDefaultCommand(new ArmTeleop(arm,
() -> ProcessedAxisValue(manipulatorController, Axis.kLeftY)));
// arm.setDefaultCommand(new ArmTeleop(arm,
// () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY)));
}

private void setBindingsDriver() {
Expand Down
27 changes: 19 additions & 8 deletions src/main/java/org/carlmontrobotics/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ public class Arm extends SubsystemBase {
private static TrapezoidProfile.State setpoint;

private TrapezoidProfile armProfile;
TrapezoidProfile.State goalState = new TrapezoidProfile.State(0, 0);// TODO: update pos later
private TrapezoidProfile.State goalState = new TrapezoidProfile.State(0, 0);// TODO: update pos later

// rad, rad/s
// public static TrapezoidProfile.State[] goalState = { new
Expand All @@ -96,14 +96,17 @@ public Arm() {
armMasterEncoder.setPositionConversionFactor(ROTATION_TO_RAD);
armMasterEncoder.setVelocityConversionFactor(ROTATION_TO_RAD);
armMasterEncoder.setZeroOffset(ENCODER_OFFSET_RAD);
armMasterEncoder.getVelocity();
// ------------------------------------------------------------
armMasterEncoder.setInverted(ENCODER_INVERTED);

armMotorFollower.follow(armMotorMaster, MOTOR_INVERTED_FOLLOWER);
armPIDMaster.setP(kP);

SmartDashboard.putNumber("set KP", 5.738);
armPIDMaster.setP(SmartDashboard.getNumber("set KP", 5.738));
armPIDMaster.setI(kI);
armPIDMaster.setD(kD);

armPIDMaster.setOutputRange(MIN_VOLTAGE, MAX_VOLTAGE);
// armPID.setTolerance(posToleranceRad, velToleranceRadPSec);

armPIDMaster.setFeedbackDevice(armMasterEncoder);
Expand All @@ -129,7 +132,8 @@ public Arm() {
sysIdTab.add("quasistatic backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
sysIdTab.add("dynamic forward", sysIdDynamic(SysIdRoutine.Direction.kForward));
sysIdTab.add("dynamic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse));

SmartDashboard.putNumber("arm initial position", goalState.position);
SmartDashboard.putNumber("set arm angle (rad)", 0);
lastArmPos = getArmPos();
lastMeasuredTime = Timer.getFPGATimestamp();
}
Expand All @@ -145,11 +149,12 @@ public void periodic() {
// ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
// armConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL , MAX_FF_ACCEL
// );
SmartDashboard.putNumber("Current Position", armMasterEncoder.getPosition());
SmartDashboard.putNumber("Current Position", getArmPos());


// smart dahsboard stuff
// SmartDashboard.putBoolean("ArmPIDAtSetpoint", armPID1.atSetpoint());
SmartDashboard.putBoolean("ArmPIDAtSetpoint", armAtSetpoint());
SmartDashboard.putNumber("Arm Goal Pos (rad)", goalState.position);
// SmartDashboard.putBoolean("ArmProfileFinished",
// armProfile.isFinished(armProfileTimer.get()));
// posToleranceRad = SmartDashboard.getNumber("Arm Tolerance Pos",
Expand All @@ -167,6 +172,7 @@ public void periodic() {
double currTime = Timer.getFPGATimestamp();
SmartDashboard.putNumber("Current Time", currTime);
//SmartDashboard.putNumber("Last Update (s)", lastMeasuredTime);
setArmTarget(SmartDashboard.getNumber("set arm angle (rad)", 0));

// when the value is different
double currentArmPos = getArmPos();
Expand Down Expand Up @@ -204,15 +210,20 @@ public void autoCancelArmCommand() {
// #region Drive Methods
private void driveArm() {
setpoint = armProfile.calculate(kDt, setpoint, goalState);
SmartDashboard.putNumber("setpoint goal (rad)", setpoint.position);
double armFeedVolts = armFeed.calculate(setpoint.position, setpoint.velocity);
if ((getArmPos() < LOWER_ANGLE_LIMIT_RAD)
|| (getArmPos() > UPPER_ANGLE_LIMIT_RAD)) {
armFeedVolts = armFeed.calculate(getArmPos(), 0);
//kg * cos(arm angle) * arm_COM_length
}
armPIDMaster.setReference(setpoint.position, CANSparkBase.ControlType.kPosition, 0, armFeedVolts);
armPIDMaster.setReference(Units.radiansToRotations(setpoint.position), CANSparkBase.ControlType.kPosition, 0, armFeedVolts);
}

/**
*
* @param targetPos in radians
*/
public void setArmTarget(double targetPos) {
targetPos = getArmClampedGoal(targetPos);

Expand All @@ -237,7 +248,7 @@ public void logMotor(SysIdRoutineLog log) {
armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput(),
Volts))
.angularVelocity(velocity.mut_replace(
armMasterEncoder.getVelocity()* (Math.PI/ 180),
armMasterEncoder.getVelocity(),
RadiansPerSecond))
.angularPosition(distance.mut_replace(
armMasterEncoder.getPosition(),
Expand Down

0 comments on commit d3778c1

Please sign in to comment.