From 6b220d24b988bf47481972d3f5789bdddd8f498b Mon Sep 17 00:00:00 2001 From: njoy3663 Date: Wed, 13 Nov 2024 18:31:10 -0800 Subject: [PATCH] arm + key bindings --- src/main/java/com/frcteam3636/frc2024/CAN.kt | 2 +- .../java/com/frcteam3636/frc2024/Robot.kt | 17 +++++++ .../frcteam3636/frc2024/subsystems/arm/Arm.kt | 31 ++++++++++++- .../frc2024/subsystems/arm/ArmIO.kt | 44 +++++++++++-------- .../frc2024/subsystems/wrist/Wrist.kt | 7 +-- 5 files changed, 76 insertions(+), 25 deletions(-) diff --git a/src/main/java/com/frcteam3636/frc2024/CAN.kt b/src/main/java/com/frcteam3636/frc2024/CAN.kt index 8926d2b..3ae6f61 100644 --- a/src/main/java/com/frcteam3636/frc2024/CAN.kt +++ b/src/main/java/com/frcteam3636/frc2024/CAN.kt @@ -39,7 +39,7 @@ enum class CTREMotorControllerId(val num: Int, val bus: String) { FrontRightDrivingMotor(4, "*"), RightArmMotor(10, "*"), LeftArmMotor(11, "*"), - WristMotor(11, "*"), + WristMotor(12, "*"), } fun TalonFX(id: CTREMotorControllerId) = TalonFX(id.num, id.bus) \ No newline at end of file diff --git a/src/main/java/com/frcteam3636/frc2024/Robot.kt b/src/main/java/com/frcteam3636/frc2024/Robot.kt index 8075559..02cfcd0 100644 --- a/src/main/java/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/java/com/frcteam3636/frc2024/Robot.kt @@ -2,6 +2,7 @@ package com.frcteam3636.frc2024 import BuildConstants import com.ctre.phoenix6.StatusSignal +import com.frcteam3636.frc2024.subsystems.arm.Arm import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain import com.frcteam3636.frc2024.subsystems.intake.Intake import edu.wpi.first.hal.FRCNetComm.tInstances @@ -143,6 +144,22 @@ object Robot : LoggedRobot() { Intake.outtake(), ) ) + + //Arm positions + controller.a() + .onTrue( + Arm.moveToPosition(Arm.Position.Stowed) + ) + + controller.x() + .onTrue( + Arm.moveToPosition(Arm.Position.PickUp) + ) + + controller.y() + .onTrue( + Arm.moveToPosition(Arm.Position.Lower) + ) } /** Add data to the driver station dashboard. */ diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index 37b86dc..530eff2 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -1,4 +1,33 @@ package com.frcteam3636.frc2024.subsystems.arm -object Arm { +import edu.wpi.first.units.Angle +import edu.wpi.first.units.Measure +import edu.wpi.first.units.Units.Degrees +import edu.wpi.first.wpilibj2.command.Subsystem +import org.littletonrobotics.junction.Logger + +object Arm : Subsystem { + var io = ArmIOReal() + + var inputs = ArmIO.ArmInputs() + + override fun periodic() { + io.updateInputs(inputs) + + Logger.processInputs("/Arm", inputs) + } + + fun moveToPosition(position: Position) = + startEnd({ + io.setPosition(position.angle) + }, { + io.setPosition(inputs.position) + }) + + + enum class Position(val angle: Measure) { + Stowed(Degrees.of(135.0)), + PickUp(Degrees.of(30.0)), + Lower(Degrees.of(-15.0)) + } } \ No newline at end of file diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index cc0ffd2..6623fb7 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -1,27 +1,23 @@ package com.frcteam3636.frc2024.subsystems.arm import com.ctre.phoenix6.configs.TalonFXConfiguration -import com.ctre.phoenix6.configs.TalonFXConfigurator +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC import com.ctre.phoenix6.signals.GravityTypeValue import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue -import edu.wpi.first.math.geometry.Rotation2d -import org.littletonrobotics.junction.LogTable -import org.littletonrobotics.junction.inputs.LoggableInputs -import com.frcteam3636.frc2024.CANSparkFlex import com.frcteam3636.frc2024.CTREMotorControllerId -import com.frcteam3636.frc2024.REVMotorControllerId import com.frcteam3636.frc2024.TalonFX -import com.revrobotics.CANSparkLowLevel -import edu.wpi.first.math.trajectory.TrapezoidProfile -import edu.wpi.first.networktables.NetworkTableInstance +import com.frcteam3636.frc2024.utils.math.MotorFFGains +import com.frcteam3636.frc2024.utils.math.PIDGains +import com.frcteam3636.frc2024.utils.math.motorFFGains +import com.frcteam3636.frc2024.utils.math.pidGains import edu.wpi.first.units.Angle import edu.wpi.first.units.Measure import edu.wpi.first.units.Units.* -import edu.wpi.first.units.Units.Rotations import edu.wpi.first.wpilibj.DigitalInput import edu.wpi.first.wpilibj.DutyCycleEncoder +import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.Logger - +import org.littletonrobotics.junction.inputs.LoggableInputs interface ArmIO{ @@ -29,8 +25,9 @@ interface ArmIO{ var rightPosition = Radians.zero() var leftPosition = Radians.zero() + var position = Radians.zero() - var absoluteEncoderPostion = Radians.zero() + var absoluteEncoderPosition = Radians.zero() var rightCurrent = Volts.zero() var leftCurrent = Volts.zero() @@ -82,9 +79,10 @@ interface ArmIO{ private val absoluteEncoder = DutyCycleEncoder(DigitalInput(7)) override fun updateInputs(inputs: ArmIO.ArmInputs) { + inputs.position = Rotations.of(absoluteEncoder.absolutePosition) inputs.absoluteEncoderConnected = absoluteEncoder.isConnected - inputs.absoluteEncoderPostion = Rotations.of(absoluteEncoder.absolutePosition) + inputs.absoluteEncoderPosition = Rotations.of(absoluteEncoder.absolutePosition) inputs.leftPosition = Rotations.of(leftMotor.position.value) inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) @@ -95,14 +93,19 @@ interface ArmIO{ inputs.rightCurrent = Volts.of(leftMotor.motorVoltage.value) + leftMotor.setPosition(inputs.absoluteEncoderPosition.`in`(Rotations)) + rightMotor.setPosition(inputs.absoluteEncoderPosition.`in`(Rotations)) } override fun setPosition(position: Measure) { Logger.recordOutput("Shooter/Pivot/Position Setpoint", position) - val leftControl = + val leftControl = MotionMagicTorqueCurrentFOC(0.0).apply { + Slot = 0 + Position = position.`in`(Rotations) + } + leftMotor.setControl(leftControl) - TODO("Not yet implemented") } init { val config = TalonFXConfiguration().apply { @@ -136,12 +139,17 @@ interface ArmIO{ config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive rightMotor.configurator.apply(config) - - leftMotor.setPosition(HARDSTOP_OFFSET.rotations) - rightMotor.setPosition(HARDSTOP_OFFSET.rotations) } + internal companion object Constants { + private const val GEAR_RATIO = 0.0 + val PID_GAINS = PIDGains(120.0, 0.0, 100.0) //placeholders + val FF_GAINS = MotorFFGains(7.8, 0.0, 0.0) //placeholders + private const val GRAVITY_GAIN = 0.0 + private const val PROFILE_ACCELERATION = 0.0 + private const val PROFILE_JERK = 0.0 + private const val PROFILE_VELOCITY = 0.0 } } \ No newline at end of file diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt index 800ebf2..54c6611 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt @@ -1,10 +1,7 @@ package com.frcteam3636.frc2024.subsystems.wrist -import com.frcteam3636.frc2024.Robot +import com.frcteam3636.frc2024.subsystems.arm.Arm import com.frcteam3636.frc2024.subsystems.intake.IntakeIO -import com.frcteam3636.frc2024.subsystems.wrist.WristIO -import edu.wpi.first.units.Units.Radians -import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger @@ -16,6 +13,6 @@ object Wrist: Subsystem { override fun periodic() { Logger.processInputs("Intake", inputs) //the wrist should always be parallel to the ground - io.pivotToAndStop(Radians.zero()) //use position variable from arm + io.pivotToAndStop(Arm.inputs.position * -1.0) //use position variable from arm } } \ No newline at end of file