feat: manual control

FRC3636 · Dec 12, 2024 · c7cae26 · c7cae26
1 parent a39e52e
commit c7cae26
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 28 deletions.
diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt
@@ -204,32 +204,37 @@ object Robot : PatchedLoggedRobot() {
 //            )
 
         //SysId
-        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)
-                )
-                    .until(Arm.inSysIdUpperRange.negate())
-            )
-
-
-        controller.a()
-            .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse))
+//        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)
+//                )
+//                    .until(Arm.inSysIdUpperRange.negate())
+//            )
+//
+//
+//        controller.a()
+//            .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse))
+//
+//        controller.rightBumper()
+//            .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward)
+////            .until(Arm.inSysIdUpperRange.negate()))
+//            )
+//        controller.leftBumper()
+//            .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse))
+//
 
-        controller.rightBumper()
-            .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward)
-//            .until(Arm.inSysIdUpperRange.negate()))
-            )
         controller.leftBumper()
-            .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse))
-
+            .whileTrue(Arm.setSpeed(0.3))
+        controller.rightBumper()
+            .whileTrue(Arm.setSpeed(-0.3))
         //Arm positions1
 //        controller.a()
 //            .onTrue(

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
@@ -72,9 +72,9 @@ object Arm : Subsystem {
         armAngleLigament.angle = inputs.position.`in`(Degrees)
         armWristAngleLigament.angle = 90.0 - inputs.position.`in`(Degrees)
 
-        if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){
-                io.updatePosition(inputs.position)
-        }
+//        if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){
+//                io.updatePosition(inputs.position)
+//        }
 
         Logger.recordOutput("/Arm/Mechanism", mechanism)
 

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
@@ -95,7 +95,8 @@ class ArmIOReal: ArmIO {
         Logger.recordOutput("/Arm/Voltage", volts)
         val control = VoltageOut(volts.`in`(Volts))
         leftMotor.setControl(control) // TODO: fix gearbox
-//        rightMotor.setControl(control)
+        control.Output *= 1.0085146641438032
+        rightMotor.setControl(control)
     }
 
     init {