Skip to content

Commit

Permalink
More changes
Browse files Browse the repository at this point in the history
tails618 committed Jan 11, 2024
1 parent 4cf70a7 commit 41bca1e
Showing 6 changed files with 36 additions and 14 deletions.
1 change: 1 addition & 0 deletions networktables.json
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
[]
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
@@ -134,6 +134,8 @@ public void teleopInit() {
ModuleConstants.TURNING_MOTOR_IDLE_MODE);
m_robotContainer.drive.backRight.turningSparkMax.setIdleMode(
ModuleConstants.TURNING_MOTOR_IDLE_MODE);

m_robotContainer.arm.initControlLoop();
}

/** This function is called periodically during operator control. */
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/RobotMap.java
Original file line number Diff line number Diff line change
@@ -29,8 +29,8 @@ public class RobotMap {
public static final int ARM_PIVOT_MOTOR_CAN_ID = 5;

/** Intake Roller Motor CAN IDs */
public static final int FRONT_INTAKE_ROLLER_MOTOR_CAN_ID = 9,
BACK_INTAKE_ROLLER_MOTOR_CAN_ID = 10;
public static final int FRONT_INTAKE_ROLLER_MOTOR_CAN_ID = 10,
BACK_INTAKE_ROLLER_MOTOR_CAN_ID = 9;

/** CANdle for the lights */
public static final int CANDLE_CAN_ID = 50;
11 changes: 9 additions & 2 deletions src/main/java/frc/robot/subsystems/arm/Arm.java
Original file line number Diff line number Diff line change
@@ -77,6 +77,12 @@ public Arm(ScoringLocationUtil scoreLoc) {

public void initialize() {
SparkMaxUtils.initWithRetry(this::initSparks, Calibrations.SPARK_INIT_RETRY_ATTEMPTS);
initControlLoop();
}

public void initControlLoop(){
armController.setTolerance(ArmCal.ARM_ALLOWED_CLOSED_LOOP_ERROR_DEG);
armController.reset(this.getArmAngle());
armController.setGoal(this.getArmAngle());
}

@@ -120,7 +126,8 @@ public void approachDesiredPosition() {
ArmCal.ARM_FEEDFORWARD.calculate(
getArmAngleRelativeToHorizontal() * (Math.PI / 180.0),
armController.getSetpoint().velocity * (Math.PI / 180.0));
// armMotor.setVoltage(armDemandVoltsA + armDemandVoltsB);
armMotor.setVoltage(armDemandVoltsA + armDemandVoltsB);
// armMotor.setVoltage(0);
SmartDashboard.putNumber("Arm PID", armDemandVoltsA);
SmartDashboard.putNumber("Arm FF", armDemandVoltsB);
mostRecentArmPID = armDemandVoltsA;
@@ -234,7 +241,7 @@ public boolean initSparks() {

// inverting stuff
errors += SparkMaxUtils.check(armAbsoluteEncoder.setInverted(true));
armMotor.setInverted(false);
armMotor.setInverted(true);

errors += setDegreesFromGearRatioAbsoluteEncoder(armAbsoluteEncoder, 26.0 / 24.0);

9 changes: 6 additions & 3 deletions src/main/java/frc/robot/subsystems/arm/ArmCal.java
Original file line number Diff line number Diff line change
@@ -15,9 +15,9 @@ public class ArmCal {
/** parameters for arm controller */
public static final double
ARM_MAX_VELOCITY_DEG_PER_SECOND =
15.0, // 5880 rpm / (60 sec/min) * (360 deg/rev) / 135.4 = 250.56
20.0, // 5880 rpm / (60 sec/min) * (360 deg/rev) / 135.4 = 250.56
ARM_MAX_ACCELERATION_DEG_PER_SECOND_SQUARED =
15.0; // angular accel = Torque / Inertia. 3.36 Nm * 135.4 / (7.26 * 0.3^2) kg-m^2 * (360
20.0; // angular accel = Torque / Inertia. 3.36 Nm * 135.4 / (7.26 * 0.3^2) kg-m^2 * (360
// deg / 2pi
// rad) = 39893 deg/s^2

@@ -38,12 +38,15 @@ public class ArmCal {
*/
public static final double ARM_MARGIN_DEGREES = 2.0;

// functional not logical
public static final double ARM_ALLOWED_CLOSED_LOOP_ERROR_DEG = 1.0;

/** Input deg/s, output volts. From recalc */
public static final ArmFeedforward ARM_FEEDFORWARD = new ArmFeedforward(0.0, 0.53, 2.63, 0.02);

public static final float ARM_NEGATIVE_LIMIT_DEGREES = 75;
public static final float ARM_POSITIVE_LIMIT_DEGREES = 275;
public static final int ARM_CURRENT_LIMIT_AMPS = 5;
public static final int ARM_CURRENT_LIMIT_AMPS = 10;

/** Various timeouts for auto lift movements */
public static final double SCORE_TO_START_FAST_SEC = PLACEHOLDER_DOUBLE,
23 changes: 16 additions & 7 deletions src/main/java/frc/robot/subsystems/grabber/Grabber.java
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
package frc.robot.subsystems.grabber;

import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxRelativeEncoder;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.DigitalInput;
@@ -18,9 +20,12 @@ public class Grabber extends SubsystemBase {
private CANSparkMax frontMotor =
new CANSparkMax(RobotMap.FRONT_INTAKE_ROLLER_MOTOR_CAN_ID, MotorType.kBrushless);

private CANSparkMax backMotor =
public CANSparkMax backMotor =
new CANSparkMax(RobotMap.BACK_INTAKE_ROLLER_MOTOR_CAN_ID, MotorType.kBrushless);

private RelativeEncoder frontEncoder = frontMotor.getEncoder();
private RelativeEncoder backEncoder = backMotor.getEncoder();

private final DigitalInput gamePieceSensor =
new DigitalInput(RobotMap.GRABBER_GAME_PIECE_SENSOR_DIO);

@@ -39,14 +44,17 @@ public void initialize() {
SparkMaxUtils.initWithRetry(this::initSparks, Calibrations.SPARK_INIT_RETRY_ATTEMPTS);
}

public void spinMotors(double power) {
public void setMotors(double power) {
runningCommand = !(Math.abs(power) < 0.01);
frontMotor.set(power);
backMotor.set(power);
System.out.println("spinning grabber motors at " + power);
System.out.println("back motor: " + backMotor.get());
}

public void intake() {
spinMotors(GrabberCalibrations.INTAKING_POWER);
setMotors(GrabberCalibrations.INTAKING_POWER);
System.out.println("intaking");
}

public void score(ScoreHeight height) {
@@ -58,15 +66,15 @@ public void score(ScoreHeight height) {
} else {
scoringPower = GrabberCalibrations.SCORE_HIGH_POWER;
}
spinMotors(scoringPower);
setMotors(scoringPower);
}

public void eject() {
spinMotors(GrabberCalibrations.EJECTION_POWER);
setMotors(GrabberCalibrations.EJECTION_POWER);
}

public void stopMotors() {
spinMotors(0.0);
setMotors(0.0);
}

public boolean seeGamePiece() {
@@ -111,11 +119,12 @@ public void initSendable(SendableBuilder builder) {
builder.addDoubleProperty("Front Motor Set Speed", frontMotor::get, frontMotor::set);
builder.addDoubleProperty("Back Motor Set Speed", backMotor::get, backMotor::set);
builder.addBooleanProperty("Sensor sees game piece", this::seeGamePiece, null);
builder.addBooleanProperty("Running command", () -> {return this.runningCommand;}, null);
}

public void periodic() {
if (seeGamePiece() && !runningCommand) {
spinMotors(GrabberCalibrations.HOLD_GAME_OBJECT_POWER);
setMotors(GrabberCalibrations.HOLD_GAME_OBJECT_POWER);
}
}
}

0 comments on commit 41bca1e

Please sign in to comment.