Skip to content

Commit

Permalink
Refactor CRServo to Servo
Browse files Browse the repository at this point in the history
  • Loading branch information
SachetK committed Nov 30, 2024
1 parent 3bc780d commit ce13986
Show file tree
Hide file tree
Showing 32 changed files with 54 additions and 76 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,4 @@ class IntakeBeltCommand(
intakeBeltSubsystem.outtakePos()
}
}

/* override fun end(interrupted: Boolean) {
intakeBeltSubsystem.stop()
}
*/}
}
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,4 @@ class IntakeCommand(
subsystem.outtake()
}
}

override fun end(interrupted: Boolean) {
subsystem.stop()
}
}
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left

import com.acmerobotics.roadrunner.geometry.Pose2d
import com.arcrobotics.ftclib.hardware.motors.CRServo
import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.OpMode
Expand All @@ -21,7 +20,7 @@ class Blue1Plus1: OpMode() {
private lateinit var elevatorLeft: Motor
private lateinit var elevatorRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo
private lateinit var intakeBelt: Servo

private lateinit var driveSubsystem: DriveSubsystem
Expand All @@ -31,7 +30,7 @@ class Blue1Plus1: OpMode() {
private lateinit var elevatorSubsystem: ElevatorSubsystem

override fun init() {
intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
Expand Down Expand Up @@ -64,11 +63,7 @@ class Blue1Plus1: OpMode() {
.addTemporalMarker(8.0) {
intakeSubsystem.outtake()
}
.waitSeconds(0.5)
.addTemporalMarker(8.5) {
intakeSubsystem.stop()
}
.waitSeconds(1.0)
.waitSeconds(1.5)
.forward(4.0)
.addTemporalMarker(9.5) {
elevatorSubsystem.setpoint = 0.0
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left

import com.arcrobotics.ftclib.hardware.motors.CRServo
import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.OpMode
import com.qualcomm.robotcore.hardware.Servo
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.constants.ControlBoard
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
Expand All @@ -18,15 +18,15 @@ class BlueHighBasketLeft: OpMode() {
private lateinit var elevatorLeft: Motor
private lateinit var elevatorRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo

private lateinit var driveSubsystem: DriveSubsystem
private lateinit var intakeSubsystem: IntakeSubsystem
private lateinit var armSubsystem: ArmSubsystem
private lateinit var elevatorSubsystem: ElevatorSubsystem

override fun init() {
intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.right

import com.arcrobotics.ftclib.hardware.motors.CRServo

import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.OpMode
import com.qualcomm.robotcore.hardware.Servo
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.constants.ControlBoard
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
Expand All @@ -13,22 +14,21 @@ import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem


@Autonomous(name = "Blue High Basket Right", group = "Basket", preselectTeleOp = "MainTeleOp")
class BlueHighBasketRight() : OpMode() {

class BlueHighBasketRight : OpMode() {
private lateinit var armLeft: Motor
private lateinit var armRight: Motor
private lateinit var elevatorLeft: Motor
private lateinit var elevatorRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo

private lateinit var driveSubsystem: DriveSubsystem
private lateinit var intakeSubsystem: IntakeSubsystem
private lateinit var armSubsystem: ArmSubsystem
private lateinit var elevatorSubsystem: ElevatorSubsystem

override fun init() {
intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
package org.firstinspires.ftc.teamcode.opModes.auto.blue.chamber.left

import com.arcrobotics.ftclib.hardware.motors.CRServo

import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.OpMode
import com.qualcomm.robotcore.hardware.Servo
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.constants.ControlBoard
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
Expand All @@ -18,15 +19,15 @@ class BlueHighChamberLeft() : OpMode() {
private lateinit var elevatorLeft: Motor
private lateinit var elevatorRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo

private lateinit var driveSubsystem: DriveSubsystem
private lateinit var intakeSubsystem: IntakeSubsystem
private lateinit var armSubsystem: ArmSubsystem
private lateinit var elevatorSubsystem: ElevatorSubsystem

override fun init() {
intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
package org.firstinspires.ftc.teamcode.opModes.auto.blue.chamber.right

import com.arcrobotics.ftclib.hardware.motors.CRServo

import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.OpMode
import com.qualcomm.robotcore.hardware.Servo
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.constants.ControlBoard
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
Expand All @@ -18,15 +19,15 @@ class BlueHighChamberRight() : OpMode() {
private lateinit var elevatorLeft: Motor
private lateinit var elevatorRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo

private lateinit var driveSubsystem: DriveSubsystem
private lateinit var intakeSubsystem: IntakeSubsystem
private lateinit var armSubsystem: ArmSubsystem
private lateinit var elevatorSubsystem: ElevatorSubsystem

override fun init() {
intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
package org.firstinspires.ftc.teamcode.opModes.auto.red.basket.left

import com.arcrobotics.ftclib.hardware.motors.CRServo

import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.OpMode
import com.qualcomm.robotcore.hardware.Servo
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.constants.ControlBoard
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
Expand All @@ -19,15 +20,15 @@ class RedHighBasketLeft() : OpMode() {
private lateinit var elevatorLeft: Motor
private lateinit var elevatorRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo

private lateinit var driveSubsystem: DriveSubsystem
private lateinit var intakeSubsystem: IntakeSubsystem
private lateinit var armSubsystem: ArmSubsystem
private lateinit var elevatorSubsystem: ElevatorSubsystem

override fun init() {
intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
package org.firstinspires.ftc.teamcode.opModes.auto.red.basket.right

import com.arcrobotics.ftclib.hardware.motors.CRServo

import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.OpMode
import com.qualcomm.robotcore.hardware.Servo
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.constants.ControlBoard
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
Expand All @@ -18,15 +19,15 @@ class RedHighBasketRight() : OpMode() {
private lateinit var elevatorLeft: Motor
private lateinit var elevatorRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo

private lateinit var driveSubsystem: DriveSubsystem
private lateinit var intakeSubsystem: IntakeSubsystem
private lateinit var armSubsystem: ArmSubsystem
private lateinit var elevatorSubsystem: ElevatorSubsystem

override fun init() {
intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
package org.firstinspires.ftc.teamcode.opModes.auto.red.chamber.left

import com.arcrobotics.ftclib.hardware.motors.CRServo

import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.OpMode
import com.qualcomm.robotcore.hardware.Servo
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.constants.ControlBoard
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
Expand All @@ -18,15 +19,15 @@ class RedHighChamberLeft() : OpMode() {
private lateinit var elevatorLeft: Motor
private lateinit var elevatorRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo

private lateinit var driveSubsystem: DriveSubsystem
private lateinit var intakeSubsystem: IntakeSubsystem
private lateinit var armSubsystem: ArmSubsystem
private lateinit var elevatorSubsystem: ElevatorSubsystem

override fun init() {
intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
package org.firstinspires.ftc.teamcode.opModes.auto.red.chamber.right

import com.arcrobotics.ftclib.hardware.motors.CRServo

import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.OpMode
import com.qualcomm.robotcore.hardware.Servo
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.constants.ControlBoard
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
Expand All @@ -18,15 +19,15 @@ class RedHighChamberRight() : OpMode() {
private lateinit var elevatorLeft: Motor
private lateinit var elevatorRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo

private lateinit var driveSubsystem: DriveSubsystem
private lateinit var intakeSubsystem: IntakeSubsystem
private lateinit var armSubsystem: ArmSubsystem
private lateinit var elevatorSubsystem: ElevatorSubsystem

override fun init() {
intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@ import com.arcrobotics.ftclib.command.RunCommand
import com.arcrobotics.ftclib.command.button.Trigger
import com.arcrobotics.ftclib.gamepad.GamepadEx
import com.arcrobotics.ftclib.gamepad.GamepadKeys
import com.arcrobotics.ftclib.hardware.motors.CRServo
import com.arcrobotics.ftclib.hardware.motors.Motor
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
import com.qualcomm.robotcore.hardware.Servo
Expand All @@ -30,7 +29,7 @@ class MainTeleOp : CommandOpMode() {
private lateinit var armLeft: Motor
private lateinit var armRight: Motor

private lateinit var intake: CRServo
private lateinit var intake: Servo
private lateinit var intakeBelt: Servo

private lateinit var slidesSubsystem: OpenElevatorSubsystem
Expand Down Expand Up @@ -69,7 +68,7 @@ class MainTeleOp : CommandOpMode() {
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_60)
armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60)

intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)

armSubsystem = OpenArmSubsystem(armRight, armLeft)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
import com.arcrobotics.ftclib.command.CommandOpMode
import com.arcrobotics.ftclib.command.RunCommand
import com.arcrobotics.ftclib.hardware.motors.CRServo

import com.qualcomm.robotcore.eventloop.opmode.TeleOp
import com.qualcomm.robotcore.hardware.Servo
import org.firstinspires.ftc.teamcode.constants.ControlBoard
Expand All @@ -15,7 +15,7 @@ import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
@TeleOp
@Config
class IntakeTuner : CommandOpMode() {
private lateinit var intake: CRServo
private lateinit var intake: Servo
private lateinit var intakeBelt: Servo

private lateinit var intakeSubsystem: IntakeSubsystem
Expand All @@ -24,7 +24,7 @@ class IntakeTuner : CommandOpMode() {
override fun initialize() {
telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)

intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)

intakeSubsystem = IntakeSubsystem(intake)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
import com.acmerobotics.roadrunner.geometry.Pose2d
import com.acmerobotics.roadrunner.util.NanoClock
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.Disabled
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
import com.qualcomm.robotcore.util.RobotLog
import org.firstinspires.ftc.robotcore.internal.system.Misc
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
import com.acmerobotics.dashboard.config.Config
import com.acmerobotics.roadrunner.geometry.Pose2d
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.Disabled
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@ import com.acmerobotics.roadrunner.profile.MotionProfileGenerator.generateSimple
import com.acmerobotics.roadrunner.profile.MotionState
import com.acmerobotics.roadrunner.util.NanoClock
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.Disabled
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
import com.qualcomm.robotcore.hardware.DcMotor
import com.qualcomm.robotcore.util.RobotLog
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
import com.acmerobotics.dashboard.config.Config
import com.acmerobotics.roadrunner.geometry.Pose2d
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.Disabled
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem

Expand Down
Loading

0 comments on commit ce13986

Please sign in to comment.