fix the wheel

ftc13100 · Nov 3, 2024 · eae297c · eae297c
1 parent b2184aa
commit eae297c
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 8 deletions.
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.commands.drive
 import com.arcrobotics.ftclib.command.CommandBase
 import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 import java.util.function.DoubleSupplier
+import kotlin.math.pow
 
 class DriveCommand(
     private val subsystem: DriveSubsystem,
@@ -17,9 +18,9 @@ class DriveCommand(
 
     override fun execute() {
         subsystem.drive(
-            leftY = zonedDrive(-leftY.asDouble, zoneVal),
-            leftX = zonedDrive(leftX.asDouble, zoneVal),
-            rightX = zonedDrive(rightX.asDouble, zoneVal),
+            leftY = zonedDrive(-leftY.asDouble, zoneVal).pow(3),
+            leftX = zonedDrive(leftX.asDouble, zoneVal).pow(3),
+            rightX = zonedDrive(rightX.asDouble, zoneVal).pow(3),
         )
     }
 

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
@@ -1,6 +1,8 @@
 package org.firstinspires.ftc.teamcode.opModes.teleOp
 
 import com.arcrobotics.ftclib.command.CommandOpMode
+import com.arcrobotics.ftclib.command.InstantCommand
+import com.arcrobotics.ftclib.command.RunCommand
 import com.arcrobotics.ftclib.gamepad.GamepadEx
 import com.arcrobotics.ftclib.gamepad.GamepadKeys
 import com.arcrobotics.ftclib.hardware.motors.Motor
@@ -42,8 +44,8 @@ class MainTeleOp: CommandOpMode() {
 
         elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
         elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
-        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
-        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
+        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_435)
+        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_435)
 
         slidesSubsystem = SlidesSubsystem(elevatorRight, elevatorLeft)
         armSubsystem = ArmSubsystem(armRight, armLeft)
@@ -53,13 +55,18 @@ class MainTeleOp: CommandOpMode() {
         spinDownCommand = SpinDownCommand(slidesSubsystem)
         armUpCommand = ArmCommand(armSubsystem, true)
         armDownCommand = ArmCommand(armSubsystem, false)
-        driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.1)
+        driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0)
 
         //operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand)
        // operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand)
         operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armUpCommand)
         operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armDownCommand)
 
         driveSubsystem.defaultCommand = driveCommand
+
+        RunCommand({
+            telemetry.addData("Arm Position: ", armSubsystem.armAngle)
+            telemetry.update()
+        }).perpetually().schedule()
     }
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt
@@ -1,9 +1,13 @@
 package org.firstinspires.ftc.teamcode.subsystems.arm
 
+import com.acmerobotics.dashboard.config.Config
 import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import kotlin.math.PI
 
+@Config
 class ArmSubsystem(
 // Here I am just declaring the motors and what they are called on our driver hub.
         private val armRight : Motor,
@@ -14,6 +18,8 @@ class ArmSubsystem(
 //Here I am making a motor group, as the arm motors are going to work together to to turn the slides.
 
     private val turnMotors = MotorGroup(armRight, armLeft)
+    val armAngle: Double
+        get() = turnMotors.positions[0] / GoBILDA.RPM_435.cpr * 2 * PI
 
     init {
         armLeft.inverted = true
@@ -32,8 +38,11 @@ class ArmSubsystem(
     }
 
     fun stop() {
-        turnMotors.set(0.0)
+        turnMotors.set(kCos * Math.cos(armAngle))
     }
 
-
+    companion object {
+        @JvmField
+        var kCos = 0.3
+    }
 }