Skip to content

Commit

Permalink
Merge branch 'new-structure' of https://github.com/ftc13100/IntoTheDe…
Browse files Browse the repository at this point in the history 
…ep-2025 into new-structure
  • Loading branch information
@AdityaP241
AdityaP241 committed Dec 26, 2024
2 parents 4fa1dc0 + f7eaf3f commit 24445f1
Show file tree
Hide file tree
Showing 2 changed files with 84 additions and 31 deletions.
82 changes: 59 additions & 23 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,14 @@ package org.firstinspires.ftc.teamcode.opModes.teleOp

import com.arcrobotics.ftclib.command.Command
import com.arcrobotics.ftclib.command.CommandOpMode
import com.arcrobotics.ftclib.command.ConditionalCommand
import com.arcrobotics.ftclib.command.InstantCommand
import com.arcrobotics.ftclib.command.RunCommand
import com.arcrobotics.ftclib.command.SelectCommand
import com.arcrobotics.ftclib.command.button.Trigger
import com.arcrobotics.ftclib.gamepad.GamepadEx
import com.arcrobotics.ftclib.gamepad.GamepadKeys
import com.arcrobotics.ftclib.kotlin.extensions.gamepad.and
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
import org.firstinspires.ftc.teamcode.commands.arm.ArmCommand
import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand
Expand Down Expand Up @@ -55,17 +58,23 @@ class MainTeleOp : CommandOpMode() {
spinUpCommand = SelectCommand(
mapOf(
OPERATOR_MODE.MANUAL to SpinUpCommand(ElevatorSubsystem),
OPERATOR_MODE.SPECIMEN to ElevatorCommand(12.0, ElevatorSubsystem),
OPERATOR_MODE.SAMPLE to ElevatorCommand(30.0, ElevatorSubsystem),
OPERATOR_MODE.SPECIMEN to ConditionalCommand(
ElevatorCommand(12.0, ElevatorSubsystem).withTimeout(2500),
ElevatorCommand(20.0, ElevatorSubsystem).withTimeout(2500)
) { ArmSubsystem.angle > Math.toRadians(80.0) },
OPERATOR_MODE.SAMPLE to ConditionalCommand(
ElevatorCommand(30.0, ElevatorSubsystem).withTimeout(2500),
ElevatorCommand(20.0, ElevatorSubsystem).withTimeout(2500)
) { ArmSubsystem.angle > Math.toRadians(80.0) },
),
this::operatorMode
)
// spinDownCommand = ElevatorCommand(0.0, ElevatorSubsystem)
spinDownCommand = SelectCommand(
mapOf(
OPERATOR_MODE.MANUAL to SpinDownCommand(ElevatorSubsystem),
OPERATOR_MODE.SAMPLE to ElevatorCommand(0.0, ElevatorSubsystem),
OPERATOR_MODE.SPECIMEN to ElevatorCommand(0.0, ElevatorSubsystem),
OPERATOR_MODE.SAMPLE to ElevatorCommand(Math.toRadians(0.0), ElevatorSubsystem).withTimeout(2500),
OPERATOR_MODE.SPECIMEN to ElevatorCommand(Math.toRadians(0.0), ElevatorSubsystem).withTimeout(2500),
),
this::operatorMode
)
Expand All @@ -74,17 +83,17 @@ class MainTeleOp : CommandOpMode() {
armUpCommand = SelectCommand(
mapOf(
OPERATOR_MODE.MANUAL to OpenArmCommand(ArmSubsystem, true),
OPERATOR_MODE.SAMPLE to ArmCommand(90.0, ArmSubsystem),
OPERATOR_MODE.SPECIMEN to ArmCommand(90.0, ArmSubsystem),
OPERATOR_MODE.SAMPLE to ArmCommand(Math.toRadians(88.0), ArmSubsystem).withTimeout(2500),
OPERATOR_MODE.SPECIMEN to ArmCommand(Math.toRadians(88.0), ArmSubsystem).withTimeout(2500),
),
this::operatorMode
)
// armDownCommand = ArmCommand(Math.toRadians(0.0), ArmSubsystem)
armDownCommand = SelectCommand(
mapOf(
OPERATOR_MODE.MANUAL to OpenArmCommand(ArmSubsystem, false),
OPERATOR_MODE.SAMPLE to ArmCommand(0.0, ArmSubsystem),
OPERATOR_MODE.SPECIMEN to ArmCommand(0.0, ArmSubsystem),
OPERATOR_MODE.SAMPLE to ArmCommand(0.0, ArmSubsystem).withTimeout(2500),
OPERATOR_MODE.SPECIMEN to ArmCommand(0.0, ArmSubsystem).withTimeout(2500),
),
this::operatorMode
)
Expand All @@ -98,13 +107,24 @@ class MainTeleOp : CommandOpMode() {

driveCommand = DriveCommand(DriveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0)

operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whenHeld(armUpCommand)
operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whenHeld(armDownCommand)

operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenHeld(spinDownCommand)
operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenHeld(spinUpCommand)
(operator.getGamepadButton(GamepadKeys.Button.DPAD_UP) and Trigger {
operatorMode == OPERATOR_MODE.MANUAL
}) .whileActiveOnce(spinUpCommand)

// operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenHeld(
(operator.getGamepadButton(GamepadKeys.Button.DPAD_UP) and Trigger {
operatorMode != OPERATOR_MODE.MANUAL
}) .whenActive(spinUpCommand)

(operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN) and Trigger {
operatorMode == OPERATOR_MODE.MANUAL
}).whileActiveOnce(spinDownCommand)

(operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN) and Trigger {
operatorMode != OPERATOR_MODE.MANUAL
}) .whenActive(spinDownCommand)

/*// operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenHeld(
// InstantCommand({
// if(operatorMode == OPERATOR_MODE.MANUAL)
// schedule(spinUpCommand)
Expand All @@ -116,9 +136,9 @@ class MainTeleOp : CommandOpMode() {
// if(operatorMode != OPERATOR_MODE.MANUAL)
// schedule(spinUpCommand)
// })
// )
// )*/
//
// operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenHeld(
/*// operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenHeld(
// InstantCommand({
// if(operatorMode == OPERATOR_MODE.MANUAL)
// schedule(spinDownCommand)
Expand All @@ -130,9 +150,25 @@ class MainTeleOp : CommandOpMode() {
// if(operatorMode != OPERATOR_MODE.MANUAL)
// schedule(spinDownCommand)
// })
// )
//
// operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whenHeld(
// )*/

(operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER) and Trigger {
operatorMode == OPERATOR_MODE.MANUAL
}) .whileActiveOnce(armUpCommand)

(operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER) and Trigger {
operatorMode != OPERATOR_MODE.MANUAL
}) .whenActive(armUpCommand)

(operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER) and Trigger {
operatorMode == OPERATOR_MODE.MANUAL
}) .whileActiveOnce(armDownCommand)

(operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER) and Trigger {
operatorMode != OPERATOR_MODE.MANUAL
}) .whenActive(armDownCommand)

/*// operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whenHeld(
// InstantCommand({
// if(operatorMode == OPERATOR_MODE.MANUAL)
// schedule(armUpCommand)
Expand All @@ -144,8 +180,8 @@ class MainTeleOp : CommandOpMode() {
// if(operatorMode != OPERATOR_MODE.MANUAL)
// schedule(armUpCommand)
// })
// )
//
// )*/
/*//
// operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whenHeld(
// InstantCommand({
// if(operatorMode == OPERATOR_MODE.MANUAL)
Expand All @@ -158,7 +194,7 @@ class MainTeleOp : CommandOpMode() {
// if(operatorMode != OPERATOR_MODE.MANUAL)
// schedule(armDownCommand)
// })
// )
// )*/

operator.getGamepadButton(GamepadKeys.Button.Y).toggleWhenPressed(
intakeCommand,
Expand All @@ -176,11 +212,11 @@ class MainTeleOp : CommandOpMode() {

driver.getGamepadButton(GamepadKeys.Button.Y).toggleWhenPressed(
InstantCommand({
DriveSubsystem.driveMultiplier = 0.9
DriveSubsystem.driveMultiplier = 0.35
driverMode = driverMode.toggle(driverMode)
}),
InstantCommand({
DriveSubsystem.driveMultiplier = 0.5
DriveSubsystem.driveMultiplier = 0.9
driverMode = driverMode.toggle(driverMode)
})
)
Expand Down
33 changes: 25 additions & 8 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,15 +23,16 @@ object IntakeSubsystem : SubsystemBase() {
private lateinit var claw: Servo
private lateinit var wrist: CRServo

private lateinit var hardwareMap: HardwareMap

private lateinit var intakeEncoder: Encoder
private lateinit var intakeTouch: TouchSensor

val isPressed: Boolean
get() = intakeTouch.isPressed

var position = Math.toRadians(90.0)
val position
get() = intakeEncoder.getPositionAndVelocity().position / 8192.0 * 2 * PI + Math.toRadians(90.0)
private set

val velocity
get() = intakeEncoder.getPositionAndVelocity().velocity / 8192.0 * 2 * PI
Expand Down Expand Up @@ -61,17 +62,24 @@ object IntakeSubsystem : SubsystemBase() {
get() = controller.atGoal()

fun initialize(hardwareMap: HardwareMap) {
this.hardwareMap = hardwareMap

claw = hardwareMap[Servo::class.java, ControlBoard.INTAKE.deviceName]
wrist = CRServo(hardwareMap, ControlBoard.INTAKE_BELT.deviceName)
intakeTouch = hardwareMap[TouchSensor::class.java, ControlBoard.INTAKE_TOUCH.deviceName]

val intakeEnc = hardwareMap[DcMotorEx::class.java, ControlBoard.INTAKE_ENCODER.deviceName]
intakeEnc.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
intakeEnc.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER

controller.setTolerance(Math.toRadians(3.0))

intakeEncoder = OverflowEncoder(RawEncoder(intakeEnc))
intakeEncoder =
OverflowEncoder(
RawEncoder(
hardwareMap[DcMotorEx::class.java, ControlBoard.INTAKE_ENCODER.deviceName]
.apply {
this.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
this.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER
}
)
)
}

fun closeClaw() {
Expand All @@ -92,7 +100,16 @@ object IntakeSubsystem : SubsystemBase() {

fun reset() {
wrist.stop()
position = Math.toRadians(90.0)
intakeEncoder =
OverflowEncoder(
RawEncoder(
hardwareMap[DcMotorEx::class.java, ControlBoard.INTAKE_ENCODER.deviceName]
.apply {
this.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
this.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER
}
)
)
}

fun operateWrist() {
Expand Down

0 comments on commit 24445f1

Please sign in to comment.