From 2d160fc8525cfd6a938bc1b055f454f1cfec95d4 Mon Sep 17 00:00:00 2001 From: 00magikarp <94652654+00magikarp@users.noreply.github.com> Date: Fri, 12 Jan 2024 20:12:45 -0500 Subject: [PATCH] continued working on telescoping arm (ioreal/constants) created sim file worked a lot on ioreal fixed depricated table command in io file removed pseudocode from main file made constants file with all variables temporarily set to 0 --- .../constants/TelescopingArmConstants.kt | 11 ++ .../TelescopingArm/TelescopingArm.kt | 74 +------- .../TelescopingArm/TelescopingArmIO.kt | 26 +-- .../TelescopingArm/TelescopingArmIOReal.kt | 167 ++++++++++++++++-- .../TelescopingArm/TelescopingArmIOSim.kt | 4 + 5 files changed, 185 insertions(+), 97 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOSim.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/TelescopingArmConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/TelescopingArmConstants.kt index 4029b4e0..096e2f59 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/TelescopingArmConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/TelescopingArmConstants.kt @@ -1,5 +1,16 @@ package com.team4099.robot2023.config.constants +import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.derived.volts + object TelescopingArmConstants { + val L_ARM_ID = 0 + val R_ARM_ID = 0 + + val SENSOR_CPR = 0.0 + val ARM_GEAR_RATIO = 0.0.inches + val LEFT_SPOOL_RADIUS = 0.volts + } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt index 08b9ac3a..a67925cd 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt @@ -1,6 +1,11 @@ package com.team4099.robot2023.subsystems.TelescopingArm import com.team4099.lib.logging.TunableNumber +import com.team4099.robot2023.config.constants.TelescopingArmConstants +import edu.wpi.first.math.controller.ElevatorFeedforward +import edu.wpi.first.math.trajectory.TrapezoidProfile +import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.inInches import org.team4099.lib.units.base.inMeters @@ -10,11 +15,6 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.inMetersPerSecondPerSecond import org.team4099.lib.units.perSecond -import com.team4099.robot2023.config.constants.TelescopingArmConstants -import edu.wpi.first.math.controller.ElevatorFeedforward -import edu.wpi.first.math.trajectory.TrapezoidProfile -import edu.wpi.first.wpilibj2.command.SubsystemBase -import org.littletonrobotics.junction.Logger class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() { val inputs = TelescopingArmIO.TelescopingArmIOInputs() @@ -276,66 +276,4 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() { fun zeroRightEncoder() { io.zeroRightEncoder() } -} - -/* -Pseudocode: -Class TelescopingArm - Method setPosition(leftSetpoint, rightSetpoint, isUnderLoad) - Calculate acceleration for left and right - Update setpoints - Set position with appropriate feedforward calculation based on load - - Method holdPosition(loaded) - Set position for left and right with feedforward calculation for zero velocity - - Method zeroLeftEncoder() - Zero the left encoder via IO - - Method zeroRightEncoder() - Zero the right encoder via IO -End Class - -extra copy: -Class TelescopingArm - Define io as TelescopingArmIO - Define inputs as TelescopingArmIOInputs - Define loadedFeedForward as ElevatorFeedforward with load constants - Define noLoadFeedForward as ElevatorFeedForward with no-load constants - Define activelyHold as boolean, set initial value to false - Define PID constants (kP, kI, kD) with tunable numbers - Define desiredState as DesiredTelescopeStates - Define constraints as TrapezoidProfile.Constraints with max velocity and acceleration - Define leftSetpoint and rightSetpoint as TrapezoidProfile.State based on current positions and velocities - - Method periodic() - Update inputs from IO - Log various inputs and states - If PID constants have changed, configure IO with new PID values - - Define limit switch properties for left and right forward and reverse limits - - Method setOpenLoop(leftPower, rightPower, useSoftLimits) - If using soft limits and limits are reached, set power to zero, else set provided power - - Define currentPosition property - Return the greater of left or right position - - Define currentState property - Determine current state based on the currentPosition within various ranges - - Method setPosition(leftSetpoint, rightSetpoint, isUnderLoad) - Calculate acceleration for left and right - Update setpoints - Set position with appropriate feedforward calculation based on load - - Method holdPosition(loaded) - Set position for left and right with feedforward calculation for zero velocity - - Method zeroLeftEncoder() - Zero the left encoder via IO - - Method zeroRightEncoder() - Zero the right encoder via IO -End Class - */ \ No newline at end of file +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIO.kt index ca7fd232..7f49cca1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIO.kt @@ -1,4 +1,4 @@ -package com.team4099.robot2022.subsystems.climber +package com.team4099.robot2023.subsystems.TelescopingArm import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.amps @@ -50,40 +50,40 @@ interface TelescopingArmIO { } override fun fromLog(table: LogTable?) { - table?.getDouble("leftPositionInches", leftPosition.inInches)?.let { + table?.get("leftPositionInches", leftPosition.inInches)?.let { leftPosition = it.inches } - table?.getDouble("rightPositionInches", rightPosition.inInches)?.let { + table?.get("rightPositionInches", rightPosition.inInches)?.let { rightPosition = it.inches } - table?.getDouble("leftVelocityMetersPerSec", leftVelocity.inMetersPerSecond)?.let { + table?.get("leftVelocityMetersPerSec", leftVelocity.inMetersPerSecond)?.let { leftVelocity = it.meters.perSecond } - table?.getDouble("rightVelocityMetersPerSec", rightVelocity.inMetersPerSecond)?.let { + table?.get("rightVelocityMetersPerSec", rightVelocity.inMetersPerSecond)?.let { rightVelocity = it.meters.perSecond } - table?.getDouble("leftStatorCurrentAmps", leftStatorCurrent.inAmperes)?.let { + table?.get("leftStatorCurrentAmps", leftStatorCurrent.inAmperes)?.let { leftStatorCurrent = it.amps } - table?.getDouble("rightStatorCurrentAmps", rightStatorCurrent.inAmperes)?.let { + table?.get("rightStatorCurrentAmps", rightStatorCurrent.inAmperes)?.let { rightStatorCurrent = it.amps } - table?.getDouble("leftSupplyCurrentAmps", leftSupplyCurrent.inAmperes)?.let { + table?.get("leftSupplyCurrentAmps", leftSupplyCurrent.inAmperes)?.let { leftSupplyCurrent = it.amps } - table?.getDouble("rightSupplyCurrentAmps", rightSupplyCurrent.inAmperes)?.let { + table?.get("rightSupplyCurrentAmps", rightSupplyCurrent.inAmperes)?.let { rightSupplyCurrent = it.amps } - table?.getDouble("leftOutputVoltage", leftOutputVoltage.inVolts)?.let { + table?.get("leftOutputVoltage", leftOutputVoltage.inVolts)?.let { leftOutputVoltage = it.volts } - table?.getDouble("rightOutputVoltage", rightOutputVoltage.inVolts)?.let { + table?.get("rightOutputVoltage", rightOutputVoltage.inVolts)?.let { rightOutputVoltage = it.volts } - table?.getDouble("leftTemperatureCelsius", leftTemperatureCelsius)?.let { + table?.get("leftTemperatureCelsius", leftTemperatureCelsius)?.let { leftTemperatureCelsius = it } - table?.getDouble("rightTemperatureCelsius", leftTemperatureCelsius)?.let { + table?.get("rightTemperatureCelsius", leftTemperatureCelsius)?.let { rightTemperatureCelsius = it } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt index bb5e3702..b910cc72 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt @@ -1,20 +1,155 @@ -package com.team4099.robot2023.subsystems.TelescopingArm - -import board.ctre.phoenix.motorcontrol.ControlMode -import board.ctre.phoenix.motorcontrol.DemandType -import board.ctre.phoenix.motorcontrol.NeutralMode -import board.ctre.phoenix.motorcontrol.can.TalonFX -import board.ctre.phoenix.motorcontrol.can.TalonFXConfiguration -import board.team4099.lib.units.base.Length -import board.team4099.lib.units.base.amps -import board.team4099.lib.units.ctreLinearMechanismSensor -import board.team4099.lib.units.derived.ElectricalPotential -import board.team4099.lib.units.derived.inVolts -import board.team4099.lib.units.derived.volts -import board.team4099.robot2022.config.constants.Constants -import board.team4099.robot2022.config.constants.Constants.Universal.CANIVORE_NAME -import board.team4099.robot2022.config.constants.TelescopingClimberConstants +package com.team4099.robot2022.subsystems.climber + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import com.team4099.robot2023.config.constants.TelescopingArmConstants +import com.team4099.robot2023.subsystems.TelescopingArm.TelescopingArmIO +import org.team4099.lib.units.base.Length +import org.team4099.lib.units.sparkMaxLinearMechanismSensor object TelescopingArmIOReal : TelescopingArmIO { + private val telescopingLeftArm: CANSparkMax = CANSparkMax( + TelescopingArmConstants.L_ARM_ID, + CANSparkMaxLowLevel.MotorType.kBrushless + ) + + private val telescopingRightArm: CANSparkMax = CANSparkMax( + TelescopingArmConstants.R_ARM_ID, + CANSparkMaxLowLevel.MotorType.kBrushless + ) + + val telescopingLeftArmSensor = + sparkMaxLinearMechanismSensor( + telescopingLeftArm, + TelescopingArmConstants.SENSOR_CPR, + TelescopingArmConstants.ARM_GEAR_RATIO, + TelescopingArmConstants.LEFT_SPOOL_RADIUS * 2 + ) + + val telescopingRightArmSensor = + sparkMaxLinearMechanismSensor( + telescopingRightArm, + TelescopingArmConstants.SENSOR_CPR, + TelescopingArmConstants.GEAR_RATIO, + TelescopingArmConstants.RIGHT_SPOOL_RADIUS * 2 + ) + + private val telescopingConfiguration: TalonFXConfiguration = TalonFXConfiguration() + + val currentPosition: Length + get() { + return if (telescopingLeftArmSensor.position > telescopingRightArmSensor.position) { + telescopingLeftArmSensor.position + } else { + telescopingRightArmSensor.position + } + } + + init { + telescopingConfiguration.slot0.kP = TelescopingArmConstants.KP + telescopingConfiguration.slot0.kI = TelescopingArmConstants.KI + telescopingConfiguration.slot0.kD = TelescopingArmConstants.KD + telescopingConfiguration.slot0.kF = TelescopingArmConstants.KFF + + telescopingRightArm.configFactoryDefault() + telescopingRightArm.clearStickyFaults() + telescopingRightArm.configAllSettings(telescopingConfiguration) + telescopingRightArm.setNeutralMode(NeutralMode.Brake) + telescopingRightArm.enableVoltageCompensation(0.0) //CHANGE CHANGE CHANGE CHANGE CHANGE CHANGE CHANGE + telescopingRightArm.inverted = true + telescopingRightArm.configForwardSoftLimitThreshold( + telescopingRightArmSensor.positionToRawUnits( + TelescopingArmConstants.FORWARD_SOFT_LIMIT + ) + ) + telescopingRightArm.configForwardSoftLimitEnable(false) + telescopingRightArm.configReverseSoftLimitThreshold( + telescopingRightArmSensor.positionToRawUnits( + TelescopingArmConstants.REVERSE_SOFT_LIMIT + ) + ) + + telescopingLeftArm.configFactoryDefault() + telescopingLeftArm.clearStickyFaults() + telescopingLeftArm.configAllSettings(telescopingConfiguration) + telescopingLeftArm.setNeutralMode(NeutralMode.Brake) + telescopingLeftArm.enableVoltageCompensation(0.0) //CHANGE CHANGE CHANGE CHANGE CHANGE CHANGE CHANGE + telescopingLeftArm.inverted = false + telescopingLeftArm.configForwardSoftLimitThreshold( + telescopingLeftArmSensor.positionToRawUnits(TelescopingArmConstants.FORWARD_SOFT_LIMIT) + ) + telescopingLeftArm.configForwardSoftLimitEnable(false) + telescopingLeftArm.configReverseSoftLimitThreshold( + telescopingLeftArmSensor.positionToRawUnits(TelescopingArmConstants.REVERSE_SOFT_LIMIT) + ) + } + + override fun updateInputs(inputs: TelescopingArmIO.TelescopingArmIOInputs) { + inputs.leftPosition = telescopingLeftArmSensor.position + inputs.rightPosition = telescopingRightArmSensor.position + + inputs.leftVelocity = telescopingLeftArmSensor.velocity + inputs.rightVelocity = telescopingRightArmSensor.velocity + + inputs.leftStatorCurrent = telescopingLeftArm.statorCurrent.amps + inputs.rightStatorCurrent = telescopingRightArm.statorCurrent.amps + + inputs.leftSupplyCurrent = telescopingLeftArm.supplyCurrent.amps + inputs.rightSupplyCurrent = telescopingRightArm.supplyCurrent.amps + + inputs.leftOutputVoltage = telescopingLeftArm.motorOutputVoltage.volts + inputs.rightOutputVoltage = telescopingRightArm.motorOutputVoltage.volts + + inputs.leftTemperatureCelcius = telescopingLeftArm.temperature + inputs.rightTemperatureCelcius = telescopingRightArm.temperature + } + + override fun zeroLeftEncoder() { + telescopingLeftArm.selectedSensorPosition = 0.0 + } + + override fun zeroRightEncoder() { + telescopingRightArm.selectedSensorPosition = 0.0 + } + + override fun setLeftOpenLoop(percentOutput: Double) { + telescopingLeftArm.set(ControlMode.PercentOutput, percentOutput) + } + + override fun setRightOpenLoop(percentOutput: Double) { + telescopingRightArm.set(ControlMode.PercentOutput, percentOutput) + } + + override fun setLeftPosition(height: Length, feedforward: ElectricalPotential) { + telescopingLeftArm.set( + ControlMode.Position, + telescopingLeftArmSensor.positionToRawUnits(height), + DemandType.ArbitraryFeedForward, + feedforward.inVolts / 12.0 + ) + } + + override fun setRightPosition(height: Length, feedforward: ElectricalPotential) { + telescopingRightArm.set( + ControlMode.Position, + telescopingRightArmSensor.positionToRawUnits(height), + DemandType.ArbitraryFeedForward, + feedforward.inVolts / 12.0 + ) + } + + override fun configPID(kP: Double, kI: Double, kD: Double) { + telescopingLeftArm.config_kP(0, kP) + telescopingLeftArm.config_kI(0, kI) + telescopingLeftArm.config_kD(0, kD) + telescopingRightArm.config_kP(0, kP) + telescopingRightArm.config_kI(0, kI) + telescopingRightArm.config_kD(0, kD) + } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOSim.kt new file mode 100644 index 00000000..b4fb328a --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOSim.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.TelescopingArm + +object TelescopingArmIOSim { +} \ No newline at end of file