From eae297c06fe2f0d1f63d5189ba54f924f4ac4126 Mon Sep 17 00:00:00 2001
From: Ishaan Ghaskadbi <ishaanghaskadbi@Ishaans-MacBook-Air.local>
Date: Sun, 3 Nov 2024 11:55:57 -0500
Subject: [PATCH] fix the wheel

---
 .../ftc/teamcode/commands/drive/DriveCommand.kt     |  7 ++++---
 .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt       | 13 ++++++++++---
 .../ftc/teamcode/subsystems/arm/ArmSubsystem.kt     | 13 +++++++++++--
 3 files changed, 25 insertions(+), 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
index 8f87198..3a64ebc 100644
--- 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
index 218af5e..461bb75 100644
--- 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,7 +55,7 @@ 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)
@@ -61,5 +63,10 @@ class MainTeleOp: CommandOpMode() {
         operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armDownCommand)
 
         driveSubsystem.defaultCommand = driveCommand
+
+        RunCommand({
+            telemetry.addData("Arm Position: ", armSubsystem.armAngle)
+            telemetry.update()
+        }).perpetually().schedule()
     }
 }
\ No newline at end of file
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
index bd247e7..05afc8b 100644
--- 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
+    }
 }
\ No newline at end of file