arm + key bindings

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)

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. */

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<Angle>) {
+        Stowed(Degrees.of(135.0)),
+        PickUp(Degrees.of(30.0)),
+        Lower(Degrees.of(-15.0))
+    }
 }

src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt

@@ -1,36 +1,33 @@
 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{
         class ArmInputs : LoggableInputs {
 
             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<Angle>) {
             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
         }
 
     }

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
     }
 }