From 41f14f5b7710fe15519ba63345d0adf7457b1538 Mon Sep 17 00:00:00 2001 From: gniederman2083 Date: Thu, 12 Dec 2024 15:42:59 -0800 Subject: [PATCH] feat: tune pid --- .../kotlin/com/frcteam3636/frc2024/Robot.kt | 69 +++++++------- .../frcteam3636/frc2024/subsystems/arm/Arm.kt | 32 +++---- .../frc2024/subsystems/arm/ArmIO.kt | 95 +++++++++++-------- 3 files changed, 105 insertions(+), 91 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt index 0d91b27..e833e6a 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt @@ -204,47 +204,46 @@ object Robot : PatchedLoggedRobot() { // ) //SysId - controller.leftBumper() - .onTrue(Commands.runOnce(SignalLogger::start)) - - controller.rightBumper() - .onTrue(Commands.runOnce(SignalLogger::stop)) +// controller.leftBumper() +// .onTrue(Commands.runOnce(SignalLogger::start)) +// +// controller.rightBumper() +// .onTrue(Commands.runOnce(SignalLogger::stop)) +// +// controller.y() +// .whileTrue( +// Commands.sequence( +// Commands.print("Starting quasistatic"), +// Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward) +// ) +// ) +// +// +// controller.a() +// .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)) +// +// controller.x() +// .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse)) +// +// controller.b() +// .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward)) - controller.y() - .whileTrue( - Commands.sequence( - Commands.print("Starting quasistatic"), - Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward) - ) - .until(Arm.inSysIdUpperRange.negate()) +// Arm positions + controller.a() + .onTrue( + Arm.moveToPosition(Arm.Position.Stowed) ) + controller.x() + .onTrue( + Arm.moveToPosition(Arm.Position.PickUp) + ) - controller.a() - .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)) - controller.rightBumper() - .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward) -// .until(Arm.inSysIdUpperRange.negate())) + controller.y() + .onTrue( + Arm.moveToPosition(Arm.Position.Lower) ) - controller.leftBumper() - .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse)) - - //Arm positions1 -// 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/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index 75b230a..27a27ac 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -2,9 +2,6 @@ package com.frcteam3636.frc2024.subsystems.arm import com.ctre.phoenix6.SignalLogger import com.frcteam3636.frc2024.Robot -import com.frcteam3636.frc2024.subsystems.intake.Intake -import com.frcteam3636.frc2024.subsystems.intake.Intake.indexerAngleLigament -import com.frcteam3636.frc2024.subsystems.intake.Intake.intakeAngleLigament import edu.wpi.first.units.Angle import edu.wpi.first.units.Measure import edu.wpi.first.units.Units.Degrees @@ -16,13 +13,10 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d import edu.wpi.first.wpilibj.util.Color import edu.wpi.first.wpilibj.util.Color8Bit import edu.wpi.first.wpilibj2.command.Subsystem -import edu.wpi.first.wpilibj2.command.button.Trigger import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism import org.littletonrobotics.junction.Logger -import kotlin.math.cos -import kotlin.math.sin private const val SECONDS_BETWEEN_ARM_UPDATES = 1.0 @@ -55,12 +49,12 @@ object Arm : Subsystem { Mechanism(io::setVoltage, null, this) ) - var inSysIdUpperRange = Trigger { - val lowerLimit = Radians.of(3.167) - inputs.position < lowerLimit - && inputs.leftPosition < lowerLimit -// && inputs.rightPosition < lowerLimit - } +// var inSysIdUpperRange = Trigger { +// val lowerLimit = Radians.of(3.167) +// inputs.position < lowerLimit +// && inputs.leftPosition < lowerLimit +//// && inputs.rightPosition < lowerLimit +// } private var timer = Timer().apply { start() @@ -69,11 +63,13 @@ object Arm : Subsystem { override fun periodic() { io.updateInputs(inputs) Logger.processInputs("/Arm", inputs) - armAngleLigament.angle = inputs.position.`in`(Degrees) - armWristAngleLigament.angle = 90.0 - inputs.position.`in`(Degrees) + armAngleLigament.angle = inputs.leftPosition.`in`(Degrees) + armWristAngleLigament.angle = 90.0 - inputs.leftPosition.`in`(Degrees) - if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){ - io.updatePosition(inputs.position) + if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) + && inputs.leftAbsoluteEncoderConnected + && inputs.rightAbsoluteEncoderConnected) { + io.updatePosition(left = inputs.leftPosition, right = inputs.rightPosition) } Logger.recordOutput("/Arm/Mechanism", mechanism) @@ -84,7 +80,7 @@ object Arm : Subsystem { startEnd({ io.pivotToPosition(position.angle) }, { - io.pivotToPosition(inputs.position) + io.pivotToPosition(inputs.leftPosition) })!! fun sysIdQuasistatic(direction: Direction) = @@ -96,6 +92,6 @@ object Arm : Subsystem { enum class Position(val angle: Measure) { Stowed(Radians.of(0.0)), PickUp(Radians.of(-0.6)), - Lower(Radians.of(0.0)) + Lower(Radians.of(-0.4)) } } \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index 714de13..49d462e 100644 --- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -22,9 +22,10 @@ import org.team9432.annotation.Logged @Logged open class ArmInputs { - var rightPosition = Radians.zero()!! + var rightRelativePosition = Radians.zero()!! + var leftRelativePosition = Radians.zero()!! var leftPosition = Radians.zero()!! - var position = Radians.zero()!! + var rightPosition = Radians.zero()!! var rightCurrent = Volts.zero()!! var leftCurrent = Volts.zero()!! @@ -32,7 +33,8 @@ open class ArmInputs { var rightVelocity = RadiansPerSecond.zero()!! var leftVelocity = RadiansPerSecond.zero()!! - var absoluteEncoderConnected = false + var leftAbsoluteEncoderConnected = false + var rightAbsoluteEncoderConnected = false } interface ArmIO { @@ -43,7 +45,7 @@ interface ArmIO { fun setVoltage(volts: Measure) - fun updatePosition(position: Measure) + fun updatePosition(left: Measure, right: Measure) } @@ -53,48 +55,53 @@ class ArmIOReal: ArmIO { private val rightMotor = TalonFX(CTREDeviceId.RightArmMotor) - private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0)) + private val leftAbsoluteEncoder = DutyCycleEncoder(DigitalInput(0)) + private val rightAbsoluteEncoder = DutyCycleEncoder(DigitalInput(1)) override fun updateInputs(inputs: ArmInputs) { - val unoffsetPosition = Rotations.of(-absoluteEncoder.get() * CHAIN_GEAR_RATIO) - inputs.position = unoffsetPosition + ZERO_OFFSET - Logger.recordOutput("/Arm/Required Offset", unoffsetPosition.negate()) -// inputs.position = Rotations.of(-absoluteEncoder.absolutePosition) + ZERO_OFFSET - inputs.absoluteEncoderConnected = absoluteEncoder.isConnected + val offsetlessLeftPosition = Rotations.of(-leftAbsoluteEncoder.get() * CHAIN_GEAR_RATIO) + inputs.leftPosition = offsetlessLeftPosition + LEFT_ZERO_OFFSET + Logger.recordOutput("/Arm/Required Left Offset", offsetlessLeftPosition.negate()) - inputs.leftPosition = Rotations.of(leftMotor.position.value) - inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) + val offsetlessRightPosition = Rotations.of(-rightAbsoluteEncoder.get() * CHAIN_GEAR_RATIO) + inputs.rightPosition = offsetlessLeftPosition + RIGHT_ZERO_OFFSET + Logger.recordOutput("/Arm/Required Right Offset", offsetlessRightPosition.negate()) + + inputs.leftRelativePosition = Rotations.of(leftMotor.position.value) + inputs.rightRelativePosition = Rotations.of(rightMotor.position.value) + inputs.leftAbsoluteEncoderConnected = leftAbsoluteEncoder.isConnected + inputs.rightAbsoluteEncoderConnected = rightAbsoluteEncoder.isConnected - inputs.rightPosition = Rotations.of(rightMotor.position.value) + inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value) - inputs.rightCurrent = Volts.of(leftMotor.motorVoltage.value) + inputs.leftCurrent = Volts.of(leftMotor.motorVoltage.value) + inputs.rightCurrent = Volts.of(rightMotor.motorVoltage.value) } - override fun updatePosition(position: Measure) { - leftMotor.setPosition(position.`in`(Rotations)) - rightMotor.setPosition(position.`in`(Rotations)) + override fun updatePosition(left: Measure, right: Measure) { + leftMotor.setPosition(left.`in`(Rotations)) + rightMotor.setPosition(right.`in`(Rotations)) } + private val pivotControl = MotionMagicTorqueCurrentFOC(0.0) override fun pivotToPosition(position: Measure) { - val resolvedPosition = position - Logger.recordOutput("Shooter/Pivot/Position Setpoint", resolvedPosition) + Logger.recordOutput("Shooter/Pivot/Position Setpoint", position) - val control = MotionMagicTorqueCurrentFOC(0.0).apply { + val control = pivotControl.apply { Slot = 0 - Position = resolvedPosition.`in`(Rotations) + Position = position.`in`(Rotations) } leftMotor.setControl(control) -// rightMotor.setControl(control) - + rightMotor.setControl(control) } override fun setVoltage(volts: Measure) { assert(volts in Volts.of(-12.0)..Volts.of(12.0)) Logger.recordOutput("/Arm/Voltage", volts) val control = VoltageOut(volts.`in`(Volts)) - leftMotor.setControl(control) // TODO: fix gearbox +// leftMotor.setControl(control) // rightMotor.setControl(control) } @@ -109,13 +116,6 @@ class ArmIOReal: ArmIO { FeedbackRotorOffset = 0.0 } - Slot0.apply { - pidGains = PID_GAINS - motorFFGains = FF_GAINS - GravityType = GravityTypeValue.Arm_Cosine - kG = GRAVITY_GAIN - } - MotionMagic.apply { MotionMagicCruiseVelocity = PROFILE_VELOCITY MotionMagicAcceleration = PROFILE_ACCELERATION @@ -128,12 +128,26 @@ class ArmIOReal: ArmIO { } config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + config.Slot0.apply { + pidGains = LEFT_PID_GAINS + motorFFGains = LEFT_FF_GAINS + GravityType = GravityTypeValue.Arm_Cosine + kG = LEFT_GRAVITY_GAIN + } leftMotor.configurator.apply( config ) config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + config.Slot0.apply { + pidGains = RIGHT_PID_GAINS + motorFFGains = RIGHT_FF_GAINS + GravityType = GravityTypeValue.Arm_Cosine + kG = RIGHT_GRAVITY_GAIN + } rightMotor.configurator.apply(config) + + } @@ -148,14 +162,18 @@ class ArmIOReal: ArmIO { private const val CHAIN_GEAR_RATIO = 1.0 / 3.0 private const val GEAR_RATIO = 125.0 / CHAIN_GEAR_RATIO - val PID_GAINS = PIDGains(64.857, 0.0, 20.319) //placeholders - val FF_GAINS = MotorFFGains(0.33157, 14.214, 0.15033) //placeholders - private const val GRAVITY_GAIN = 0.28233 + val LEFT_PID_GAINS = PIDGains(64.138, 0.0, 59.411) + val LEFT_FF_GAINS = MotorFFGains(0.3289, 42.817, 0.39107) + private const val LEFT_GRAVITY_GAIN = 0.17119 + val RIGHT_PID_GAINS = PIDGains(63.339, 0.0, 59.134) + val RIGHT_FF_GAINS = MotorFFGains(0.29364, 43.77, 0.34751) + private const val RIGHT_GRAVITY_GAIN = 0.12181 private const val PROFILE_ACCELERATION = 1.0 private const val PROFILE_JERK = 1.0 private const val PROFILE_VELOCITY = 1.0 - val ZERO_OFFSET = Radians.of(1.01) + val LEFT_ZERO_OFFSET = Radians.of(1.05) + val RIGHT_ZERO_OFFSET = Radians.of(1.05) } } @@ -180,7 +198,8 @@ class ArmIOSim : ArmIO { armSim.update(Robot.period) inputs.rightVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) inputs.leftVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) - inputs.position = Radians.of(armSim.angleRads) + inputs.leftPosition = Radians.of(armSim.angleRads) + inputs.rightPosition = Radians.of(armSim.angleRads) // val pidVoltage = pidController.calculate(inputs.position.`in`(Radians),setPoint) // armSim.setInputVoltage(pidVoltage) } @@ -194,7 +213,7 @@ class ArmIOSim : ArmIO { Logger.recordOutput("/Arm/OutVolt", volts) } - override fun updatePosition(position: Measure) { + override fun updatePosition(left: Measure, right: Measure) { // no drifting in sim so no need to update } } @@ -210,6 +229,6 @@ class ArmIOPrototype: ArmIO { override fun setVoltage(volts: Measure) { } - override fun updatePosition(position: Measure) { + override fun updatePosition(left: Measure, right: Measure) { } }