Skip to content
This repository has been archived by the owner on Dec 19, 2024. It is now read-only.

Commit

Permalink
feat: tune pid
Browse files Browse the repository at this point in the history
  • Loading branch information
@Gavin-Niederman
Gavin-Niederman committed Dec 12, 2024
1 parent 9edc483 commit 41f14f5
Show file tree
Hide file tree
Showing 3 changed files with 105 additions and 91 deletions.
69 changes: 34 additions & 35 deletions src/main/kotlin/com/frcteam3636/frc2024/Robot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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. */
Expand Down
32 changes: 14 additions & 18 deletions src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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()
Expand All @@ -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)
Expand All @@ -84,7 +80,7 @@ object Arm : Subsystem {
startEnd({
io.pivotToPosition(position.angle)
}, {
io.pivotToPosition(inputs.position)
io.pivotToPosition(inputs.leftPosition)
})!!

fun sysIdQuasistatic(direction: Direction) =
Expand All @@ -96,6 +92,6 @@ object Arm : Subsystem {
enum class Position(val angle: Measure<Angle>) {
Stowed(Radians.of(0.0)),
PickUp(Radians.of(-0.6)),
Lower(Radians.of(0.0))
Lower(Radians.of(-0.4))
}
}
95 changes: 57 additions & 38 deletions src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,17 +22,19 @@ 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()!!

var rightVelocity = RadiansPerSecond.zero()!!
var leftVelocity = RadiansPerSecond.zero()!!

var absoluteEncoderConnected = false
var leftAbsoluteEncoderConnected = false
var rightAbsoluteEncoderConnected = false
}

interface ArmIO {
Expand All @@ -43,7 +45,7 @@ interface ArmIO {

fun setVoltage(volts: Measure<Voltage>)

fun updatePosition(position: Measure<Angle>)
fun updatePosition(left: Measure<Angle>, right: Measure<Angle>)
}


Expand All @@ -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<Angle>) {
leftMotor.setPosition(position.`in`(Rotations))
rightMotor.setPosition(position.`in`(Rotations))
override fun updatePosition(left: Measure<Angle>, right: Measure<Angle>) {
leftMotor.setPosition(left.`in`(Rotations))
rightMotor.setPosition(right.`in`(Rotations))
}

private val pivotControl = MotionMagicTorqueCurrentFOC(0.0)
override fun pivotToPosition(position: Measure<Angle>) {
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<Voltage>) {
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)
}

Expand All @@ -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
Expand All @@ -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)


}


Expand All @@ -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)
}

}
Expand All @@ -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)
}
Expand All @@ -194,7 +213,7 @@ class ArmIOSim : ArmIO {
Logger.recordOutput("/Arm/OutVolt", volts)
}

override fun updatePosition(position: Measure<Angle>) {
override fun updatePosition(left: Measure<Angle>, right: Measure<Angle>) {
// no drifting in sim so no need to update
}
}
Expand All @@ -210,6 +229,6 @@ class ArmIOPrototype: ArmIO {
override fun setVoltage(volts: Measure<Voltage>) {
}

override fun updatePosition(position: Measure<Angle>) {
override fun updatePosition(left: Measure<Angle>, right: Measure<Angle>) {
}
}

0 comments on commit 41f14f5

Please sign in to comment.