From 41bca1ebb197f4a984d97681918fc1b8830a2f77 Mon Sep 17 00:00:00 2001 From: Asher Date: Wed, 10 Jan 2024 18:41:01 -0600 Subject: [PATCH] More changes --- networktables.json | 1 + src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/RobotMap.java | 4 ++-- .../java/frc/robot/subsystems/arm/Arm.java | 11 +++++++-- .../java/frc/robot/subsystems/arm/ArmCal.java | 9 +++++--- .../frc/robot/subsystems/grabber/Grabber.java | 23 +++++++++++++------ 6 files changed, 36 insertions(+), 14 deletions(-) create mode 100644 networktables.json diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b2b923f..cb6d66d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -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. */ diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index ce9c7cd..0a8470b 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -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; diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index fb81eae..6cd36ec 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -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); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCal.java b/src/main/java/frc/robot/subsystems/arm/ArmCal.java index 7f73896..24c9d73 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCal.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCal.java @@ -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, diff --git a/src/main/java/frc/robot/subsystems/grabber/Grabber.java b/src/main/java/frc/robot/subsystems/grabber/Grabber.java index 9612940..4f7380b 100644 --- a/src/main/java/frc/robot/subsystems/grabber/Grabber.java +++ b/src/main/java/frc/robot/subsystems/grabber/Grabber.java @@ -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); } } }