Manipulator done

FRC3636 · Jan 23, 2025 · 2e2ebfa · 2e2ebfa
1 parent c5360c8
commit 2e2ebfa
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 8 deletions.
diff --git a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt
@@ -2,6 +2,7 @@ package com.frcteam3636.frc2025
 
 import com.ctre.phoenix6.StatusSignal
 import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain
+import com.frcteam3636.frc2025.subsystems.manipulator.Manipulator
 import com.frcteam3636.frc2025.utils.Elastic
 import com.frcteam3636.frc2025.utils.ElasticNotification
 import com.frcteam3636.frc2025.utils.NotificationLevel
@@ -127,6 +128,7 @@ object Robot : LoggedRobot() {
     /** Start robot subsystems so that their periodic tasks are run */
     private fun configureSubsystems() {
         Drivetrain.register()
+        Manipulator.register()
     }
 
     /** Expose commands for autonomous routines to use and display an auto picker in Shuffleboard. */

diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/manipulator/Manipulator.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/manipulator/Manipulator.kt
@@ -1,12 +1,17 @@
 package com.frcteam3636.frc2025.subsystems.manipulator
 
 import com.frcteam3636.frc2025.Robot
+import edu.wpi.first.units.Units.DegreesPerSecond
 import edu.wpi.first.units.Units.Meters
+import edu.wpi.first.wpilibj.util.Color
+import edu.wpi.first.wpilibj.util.Color8Bit
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Commands
 import edu.wpi.first.wpilibj2.command.Subsystem
 import edu.wpi.first.wpilibj2.command.button.Trigger
 import org.littletonrobotics.junction.Logger
+import org.littletonrobotics.junction.mechanism.LoggedMechanism2d
+import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d
 
 object Manipulator: Subsystem {
     private val io: ManipulatorIO = when (Robot.model){
@@ -17,13 +22,23 @@ object Manipulator: Subsystem {
 
     var inputs = LoggedManipulatorInputs()
 
+    private var mechanism = LoggedMechanism2d(100.0, 100.0)
+    private var motorAngleVisualizer = LoggedMechanismLigament2d("Manipulator Motor Angle", 40.0, 0.0, 5.0, Color8Bit(Color.kRed))
+
+    init {
+        mechanism.getRoot("Manipulator", 50.0, 50.0).apply { append(motorAngleVisualizer) }
+    }
+
     override fun periodic() {
         io.updateInputs(inputs)
         Logger.processInputs("Manipulator", inputs)
+
+        motorAngleVisualizer.angle += inputs.velocity.`in`(DegreesPerSecond) * Robot.period
+        Logger.recordOutput("/Manipulator/Mechanism", mechanism)
     }
 
-    val coralInIntakeBack = Trigger{inputs.backUltrasonicDistance < Meters.zero()}
-    val coralInIntakeFront = Trigger{inputs.frontUltrasonicDistance < Meters.zero()}
+    private val coralInIntakeBack = Trigger{inputs.backUltrasonicDistance < Meters.zero()}
+    private val coralInIntakeFront = Trigger{inputs.frontUltrasonicDistance < Meters.zero()}
 
     fun intake(): Command = startEnd(
         { io.setSpeed(0.5) },

diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/manipulator/ManipulatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/manipulator/ManipulatorIO.kt
@@ -4,10 +4,10 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration
 import com.ctre.phoenix6.signals.InvertedValue
 import com.ctre.phoenix6.signals.NeutralModeValue
 import com.frcteam3636.frc2025.CTREDeviceId
+import com.frcteam3636.frc2025.Robot
 import com.frcteam3636.frc2025.TalonFX
 import edu.wpi.first.math.system.plant.DCMotor
 import edu.wpi.first.math.system.plant.LinearSystemId
-import edu.wpi.first.math.system.plant.LinearSystemId.createDCMotorSystem
 import edu.wpi.first.units.Units.*
 import edu.wpi.first.wpilibj.Ultrasonic
 import edu.wpi.first.wpilibj.simulation.FlywheelSim
@@ -51,6 +51,7 @@ class ManipulatorIOReal: ManipulatorIO {
     override fun updateInputs(inputs: ManipulatorInputs) {
         inputs.velocity = manipulatorMotor.velocity.value
         inputs.current = manipulatorMotor.supplyCurrent.value
+
         inputs.backUltrasonicDistance = Meters.of(backUltrasonic.rangeMM * 1000)
         inputs.frontUltrasonicDistance = Meters.of(frontUltrasonic.rangeMM * 1000)
     }
@@ -65,16 +66,19 @@ class ManipulatorIOReal: ManipulatorIO {
 }
 
 class ManipulatorIOSim: ManipulatorIO {
-    var motor = DCMotor.getKrakenX60Foc(1)
-    var system = LinearSystemId.createFlywheelSystem(motor,1.0, 1.0)
-    private var simMotor = FlywheelSim(system, motor, 1.0)
+    private var motor = DCMotor.getKrakenX60Foc(1)
+    private var system = LinearSystemId.createFlywheelSystem(motor,1.0, 1.0)
+    private var simMotor = FlywheelSim(system, motor, 0.0)
 
     override fun setSpeed(percent: Double) {
-        simMotor.setInput()
+        simMotor.inputVoltage = percent * 12
     }
 
     override fun updateInputs(inputs: ManipulatorInputs) {
-        TODO("Not yet implemented")
+        simMotor.update(Robot.period)
+        inputs.velocity = simMotor.angularVelocity
+        simMotor.setAngularVelocity(simMotor.angularVelocityRadPerSec * 0.95)
+        inputs.current = Amps.of(simMotor.currentDrawAmps)
     }
 
 }