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 a1af62e2..47cea5ec 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt @@ -1,4 +1,40 @@ package com.team4099.robot2023.subsystems.TelescopingArm +import com.team4099.lib.logging.TunableNumber +import board.team4099.lib.units.base.Length +import board.team4099.lib.units.base.inInches +import board.team4099.lib.units.base.inMeters +import board.team4099.lib.units.base.meters +import board.team4099.lib.units.derived.inVolts +import board.team4099.lib.units.derived.volts +import board.team4099.lib.units.inMetersPerSecond +import board.team4099.lib.units.inMetersPerSecondPerSecond +import board.team4099.lib.units.perSecond +import board.team4099.robot2022.config.constants.TelescopingClimberConstants +import board.team4099.robot2022.config.constants.TelescopingClimberConstants.ActualTelescopeStates +import board.team4099.robot2022.config.constants.TelescopingClimberConstants.DesiredTelescopeStates +import board.team4099.robot2022.config.constants.TelescopingClimberConstants.telescopingTolerance +import edu.wpi.first.math.controller.ElevatorFeedforward +import edu.wpi.first.math.trajectory.TrapezoidProfile +import edu.wpi.first.wpilibj2.boardmand.SubsystemBase +import org.littletonrobotics.junction.Logger class TelescopingArm { + + fun holdPosition(loaded: Boolean = true) { + if (loaded) { + io.setLeftPosition(inputs.leftPosition, loadedFeedForward.calculate(0.0).volts) + io.setRightPosition(inputs.rightPosition, loadedFeedForward.calculate(0.0).volts) + } else { + io.setLeftPosition(inputs.leftPosition, noLoadFeedForward.calculate(0.0).volts) + io.setRightPosition(inputs.rightPosition, noLoadFeedForward.calculate(0.0).volts) + } + } + + fun zeroLeftEncoder() { + io.zeroLeftEncoder() + } + + fun zeroRightEncoder() { + io.zeroRightEncoder() + } } \ 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 7d257b71..c6a2ace8 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIO.kt @@ -1,5 +1,19 @@ package com.team4099.robot2023.subsystems.TelescopingArm -class TelescopingArm { - +import board.team4099.lib.units.base.Length +import board.team4099.lib.units.base.amps +import board.team4099.lib.units.base.inAmperes +import board.team4099.lib.units.base.inInches +import board.team4099.lib.units.base.inches +import board.team4099.lib.units.base.meters +import board.team4099.lib.units.derived.ElectricalPotential +import board.team4099.lib.units.derived.inVolts +import board.team4099.lib.units.derived.volts +import board.team4099.lib.units.inMetersPerSecond +import board.team4099.lib.units.perSecond +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs + +class TelescopingArmIO { + } \ No newline at end of file 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 c6429f36..596d6915 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt @@ -1,4 +1,19 @@ 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 class TelescopingArmIOReal { + } \ No newline at end of file