sachet

ftc13100 · Nov 28, 2024 · 4a50493 · 4a50493
1 parent e0f0659
commit 4a50493
Show file tree

Hide file tree

Showing 14 changed files with 35 additions and 21 deletions.
diff --git a/...kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt b/...kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt
@@ -37,7 +37,7 @@ class BlueHighBasketLeft: OpMode() {
         driveSubsystem = DriveSubsystem(hardwareMap)
 //        intakeSubsystem = IntakeSubsystem(intake)
         armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
         val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
             .turn(Math.toRadians(-90.0))

diff --git a/...tlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt b/...tlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt
@@ -39,7 +39,7 @@ class BlueHighBasketRight() : OpMode() {
         driveSubsystem = DriveSubsystem(hardwareMap)
         intakeSubsystem = IntakeSubsystem(intake)
         armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
         val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose)
             .turn(Math.toRadians(90.0))

diff --git a/...tlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt b/...tlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt
@@ -37,7 +37,7 @@ class BlueHighChamberLeft() : OpMode() {
         driveSubsystem = DriveSubsystem(hardwareMap)
         intakeSubsystem = IntakeSubsystem(intake)
         armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
         val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
 

diff --git a/...in/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt b/...in/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt
@@ -37,7 +37,7 @@ class BlueHighChamberRight() : OpMode() {
         driveSubsystem = DriveSubsystem(hardwareMap)
         intakeSubsystem = IntakeSubsystem(intake)
         armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
         val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose)
 

diff --git a/...n/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt b/...n/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt
@@ -38,7 +38,7 @@ class RedHighBasketLeft() : OpMode() {
         driveSubsystem = DriveSubsystem(hardwareMap)
         intakeSubsystem = IntakeSubsystem(intake)
         armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
         val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose)
             .turn(Math.toRadians(90.0))

diff --git a/...kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt b/...kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt
@@ -37,7 +37,7 @@ class RedHighBasketRight() : OpMode() {
         driveSubsystem = DriveSubsystem(hardwareMap)
         intakeSubsystem = IntakeSubsystem(intake)
         armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
         val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose)
             .turn(Math.toRadians(90.0))

diff --git a/...kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt b/...kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt
@@ -37,7 +37,7 @@ class RedHighChamberLeft() : OpMode() {
         driveSubsystem = DriveSubsystem(hardwareMap)
         intakeSubsystem = IntakeSubsystem(intake)
         armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
         val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose)
 

diff --git a/...tlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt b/...tlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt
@@ -37,7 +37,7 @@ class RedHighChamberRight() : OpMode() {
         driveSubsystem = DriveSubsystem(hardwareMap)
         intakeSubsystem = IntakeSubsystem(intake)
         armSubsystem = ArmSubsystem(armRight, armLeft)
-        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft)
+        elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
         val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose)
 

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
@@ -54,8 +54,8 @@ class MainTeleOp : CommandOpMode() {
 
         //intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
 
-        slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft)
         armSubsystem = OpenArmSubsystem(armRight, armLeft)
+        slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
         driveSubsystem = DriveSubsystem(hardwareMap)
         //intakeSubsystem = IntakeSubsystem(intake)
 
@@ -80,7 +80,7 @@ class MainTeleOp : CommandOpMode() {
         driveSubsystem.defaultCommand = driveCommand
 
         RunCommand({
-            telemetry.addData("Arm Position: ", armSubsystem.armAngle)
+            telemetry.addData("Arm Position: ", Math.toDegrees(armSubsystem.armAngle))
             telemetry.update()
         }).perpetually().schedule()
     }

diff --git a/...ain/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt b/...ain/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt
@@ -26,7 +26,7 @@ class SlidesConstraintsTuner : LinearOpMode() {
         leftMotor = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
         rightMotor = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
 
-        elevator = ElevatorSubsystem(rightMotor, leftMotor)
+        elevator = ElevatorSubsystem(rightMotor, leftMotor) { 0.0 }
         waitForStart()
 
         var dt = 0L

diff --git a/...de/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt b/...de/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt
@@ -8,23 +8,32 @@ import com.arcrobotics.ftclib.command.RunCommand
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 @TeleOp
 @Config
 class SlidesPIDTuner : CommandOpMode() {
+    private lateinit var slidesLeft: Motor
+    private lateinit var slidesRight: Motor
+
     private lateinit var armLeft: Motor
     private lateinit var armRight: Motor
 
+    private lateinit var armSubsystem: ArmSubsystem
     private lateinit var slidesSubsystem: ElevatorSubsystem
 
     override fun initialize() {
         telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
 
-        armLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150)
-        armRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150)
+        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60)
+        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_60)
+
+        slidesLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150)
+        slidesRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150)
 
-        slidesSubsystem = ElevatorSubsystem(armRight, armLeft)
+        armSubsystem = ArmSubsystem(armRight, armLeft)
+        slidesSubsystem = ElevatorSubsystem(slidesRight, slidesLeft, armSubsystem::armAngle)
 
         RunCommand({
             slidesSubsystem.setpoint = target

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt
@@ -5,21 +5,22 @@ 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 org.firstinspires.ftc.teamcode.constants.ArmConstants
 import kotlin.math.PI
+import kotlin.math.cos
 
 @Config
 class OpenArmSubsystem(
 // Here I am just declaring the motors and what they are called on our driver hub.
         armRight : Motor,
         armLeft : Motor,
-
 ) : SubsystemBase() {
 
 //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_312.cpr * 2 * PI
+        get() = turnMotors.positions[0] / GoBILDA.RPM_60.cpr * PI
 
     init {
         armLeft.inverted = true
@@ -38,7 +39,7 @@ class OpenArmSubsystem(
     }
 
     fun stop() {
-        turnMotors.set(kCos * Math.cos(armAngle))
+        turnMotors.set(ArmConstants.kCos.value * cos(armAngle))
     }
 
     companion object {

diff --git a/...ode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt b/...ode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt
@@ -7,11 +7,13 @@ import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
 import com.arcrobotics.ftclib.trajectory.TrapezoidProfile
 import org.firstinspires.ftc.teamcode.constants.SlidesConstants
+import kotlin.math.sin
 
 @Config
 class ElevatorSubsystem(
     elevatorRight : Motor,
-    elevatorLeft : Motor
+    elevatorLeft : Motor,
+    private val slideAngle: () -> Double
 ) : SubsystemBase() {
     private val extendMotors = MotorGroup(elevatorRight, elevatorLeft)
     private val controller = ProfiledPIDController(
@@ -50,7 +52,7 @@ class ElevatorSubsystem(
         val output = controller.calculate(slidePos)
 
         if (enabled)
-            extendMotors.set(output + SlidesConstants.kG.value)
+            extendMotors.set(output + SlidesConstants.kG.value * sin(slideAngle.invoke()))
     }
 
     fun toggle() {

diff --git a/...src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt b/...src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt
@@ -4,11 +4,13 @@ import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
 import org.firstinspires.ftc.teamcode.constants.SlidesConstants
+import kotlin.math.sin
 
 class OpenElevatorSubsystem(
     //sets them as a private variable thats a motor (same name as in driver hub)
     elevatorRight : Motor,
-    elevatorLeft : Motor
+    elevatorLeft : Motor,
+    private val slideAngle: () -> Double
 ) : SubsystemBase() {
     //makes a motor group bc you have to move them at the same time
     private val elevatorMotors = MotorGroup(elevatorRight, elevatorLeft)
@@ -27,7 +29,7 @@ class OpenElevatorSubsystem(
     }
 
     fun stop() {
-        elevatorMotors.set(SlidesConstants.kG.value)
+        elevatorMotors.set(SlidesConstants.kG.value * sin(slideAngle.invoke()))
     }
 
 }