hardware map

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt

@@ -4,5 +4,5 @@ enum class ArmConstants(val value: Double) {
     kP(1.0),
     kI(0.0),
     kD(0.08),
-    kCos(0.0045)
+    kCos(0.01)
 }

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt

@@ -20,8 +20,8 @@ enum class ControlBoard(val deviceName: String) {
     SLIDES_RIGHT("rightSlideString"),
 
     //Intake
-    INTAKE(""),
+    INTAKE("rollerIntake"),
+    INTAKE_BELT("intakeBelt")
+
 
-    // Camera
-    CAMERA("lifeCam")
 }

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt

@@ -26,7 +26,7 @@ class BlueHighBasketLeft: OpMode() {
     private lateinit var elevatorSubsystem: ElevatorSubsystem
 
     override fun init() {
-//        intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
+        intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
 
         armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
         armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
@@ -35,29 +35,31 @@ class BlueHighBasketLeft: OpMode() {
         elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
 
         driveSubsystem = DriveSubsystem(hardwareMap)
-//        intakeSubsystem = IntakeSubsystem(intake)
+        intakeSubsystem = IntakeSubsystem(intake)
         armSubsystem = ArmSubsystem(armRight, armLeft)
         elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
         val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
             .turn(Math.toRadians(-90.0))
             .back(60.0)
+            .turn(Math.toRadians(20.0))
+            .back(5.0)
             .addTemporalMarker(3.0) {
                 armSubsystem.setpoint = Math.toRadians(95.0)
             }
             .waitSeconds(1.0)
-            .addTemporalMarker(6.0) {
-                elevatorSubsystem.setpoint = 1000.0
+            .addTemporalMarker(5.0) {
+                elevatorSubsystem.setpoint = 1900.0
             }
             .waitSeconds(2.0)
-//            .addTemporalMarker(5.0){
-//                intakeSubsystem.outtake()
-//            }
+            .addTemporalMarker(6.0){
+                intakeSubsystem.outtake()
+            }
             .waitSeconds(1.0)
-            .addTemporalMarker(10.0) {
-                elevatorSubsystem.setpoint = 50.0
+            .addTemporalMarker(9.0) {
+                elevatorSubsystem.setpoint = 0.0
             }
-            .waitSeconds(2.0)
+            .waitSeconds(5.0)
             .addTemporalMarker(13.0) {
                 armSubsystem.setpoint = Math.toRadians(5.0)
             }

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt

@@ -44,7 +44,7 @@ class BlueHighChamberLeft() : OpMode() {
             .forward(28.0)
             .strafeLeft(11.0)
             .addTemporalMarker(2.0) {
-                armSubsystem.setpoint = Math.toRadians(150.0)
+                armSubsystem.setpoint = Math.toRadians(.0)
             }
             .waitSeconds(1.0)
             .addTemporalMarker(3.0) {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt

@@ -43,6 +43,7 @@ class SlidesPIDTuner : CommandOpMode() {
             telemetry.addData("Slides Position", slidesSubsystem.slidePos)
             telemetry.addData("Setpoint", target)
             telemetry.addData("arm angle", armSubsystem.armAngle)
+            telemetry.addData("slides Velocity", slidesSubsystem.slideVelocity)
             telemetry.update()
         }).perpetually().schedule()
 

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.subsystems.intake
+
+import com.arcrobotics.ftclib.command.SubsystemBase
+import com.qualcomm.robotcore.hardware.Servo
+
+class IntakeBeltSubsytem(
+    private val intakeBelt: Servo
+) : SubsystemBase() {
+    fun intake() { intakeBelt.position = 0.5 }
+
+    fun outtake() { intakeBelt.position = 0.0 }
+}

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt

@@ -21,8 +21,8 @@ class ElevatorSubsystem(
         SlidesConstants.kI.value,
         SlidesConstants.kD.value,
         TrapezoidProfile.Constraints(
-            2000.0,
-            1000.0
+            1600.0,
+            700.0
         )
     )
 
@@ -45,7 +45,7 @@ class ElevatorSubsystem(
         elevatorRight.encoder.setDirection(Motor.Direction.REVERSE)
 
         extendMotors.resetEncoder()
-        extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
+        extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT)
     }
 
     override fun periodic() {
@@ -71,7 +71,8 @@ class ElevatorSubsystem(
 
     companion object {
         @JvmField
-        var kP = 0.014
+        var kP = 0.01
+        // kP = 0.01
 
         @JvmField
         var kI = 0.0
@@ -81,5 +82,8 @@ class ElevatorSubsystem(
 
         @JvmField
         var kG = 0.115
+        // kG = 0.115
+
     }
-}
+}
+