Skip to content

Commit

Permalink
Swap motors
Browse files Browse the repository at this point in the history
  • Loading branch information
@SachetK
SachetK committed Nov 28, 2024
1 parent 0c350a5 commit 6b6bad1
Show file tree
Hide file tree
Showing 10 changed files with 33 additions and 48 deletions.
20 changes: 10 additions & 10 deletions ...kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem

@Autonomous(name = "Blue High Basket Left", group = "Basket", preselectTeleOp = "MainTeleOp")
class BlueHighBasketLeft() : OpMode() {
class BlueHighBasketLeft: OpMode() {
private lateinit var armLeft: Motor
private lateinit var armRight: Motor
private lateinit var elevatorLeft: Motor
Expand All @@ -26,7 +26,7 @@ class BlueHighBasketLeft() : OpMode() {
private lateinit var elevatorSubsystem: SlidesSubsytem

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)
Expand All @@ -35,9 +35,9 @@ class BlueHighBasketLeft() : OpMode() {
elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)

driveSubsystem = DriveSubsystem(hardwareMap)
//intakeSubsystem = IntakeSubsystem(intake)
armSubsystem = ArmSubsystem(armLeft, armRight)
elevatorSubsystem = SlidesSubsytem(elevatorLeft, elevatorRight)
// intakeSubsystem = IntakeSubsystem(intake)
armSubsystem = ArmSubsystem(armRight, armLeft)
elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft)

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
.turn(Math.toRadians(90.0))
Expand All @@ -50,9 +50,9 @@ class BlueHighBasketLeft() : OpMode() {
elevatorSubsystem.setpoint = 250.0
}
.waitSeconds(2.0)
//.addTemporalMarker(5.0){
//intakeSubsystem.outtake()
//}
// .addTemporalMarker(5.0){
// intakeSubsystem.outtake()
// }
.waitSeconds(1.0)
.addTemporalMarker(10.0) {
elevatorSubsystem.setpoint = 0.0
Expand All @@ -62,14 +62,14 @@ class BlueHighBasketLeft() : OpMode() {
armSubsystem.setpoint = Math.toRadians(0.0)
}
.build()
driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose


driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
driveSubsystem.followTrajectorySequenceAsync(trajectory)
}

override fun loop() {
driveSubsystem.update()

elevatorSubsystem.periodic()
armSubsystem.periodic()
}
Expand Down
4 changes: 2 additions & 2 deletions ...tlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,8 @@ class BlueHighBasketRight() : OpMode() {

driveSubsystem = DriveSubsystem(hardwareMap)
intakeSubsystem = IntakeSubsystem(intake)
armSubsystem = ArmSubsystem(armLeft, armRight)
elevatorSubsystem = SlidesSubsytem(elevatorLeft, elevatorRight)
armSubsystem = ArmSubsystem(armRight, armLeft)
elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft)

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose)
.turn(Math.toRadians(90.0))
Expand Down
4 changes: 2 additions & 2 deletions ...tlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@ class BlueHighChamberLeft() : OpMode() {

driveSubsystem = DriveSubsystem(hardwareMap)
intakeSubsystem = IntakeSubsystem(intake)
armSubsystem = ArmSubsystem(armLeft, armRight)
elevatorSubsystem = SlidesSubsytem(elevatorLeft, elevatorRight)
armSubsystem = ArmSubsystem(armRight, armLeft)
elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft)

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)

Expand Down
4 changes: 2 additions & 2 deletions ...in/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@ class BlueHighChamberRight() : OpMode() {

driveSubsystem = DriveSubsystem(hardwareMap)
intakeSubsystem = IntakeSubsystem(intake)
armSubsystem = ArmSubsystem(armLeft, armRight)
elevatorSubsystem = SlidesSubsytem(elevatorLeft, elevatorRight)
armSubsystem = ArmSubsystem(armRight, armLeft)
elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft)

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose)

Expand Down
4 changes: 2 additions & 2 deletions ...n/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,8 @@ class RedHighBasketLeft() : OpMode() {

driveSubsystem = DriveSubsystem(hardwareMap)
intakeSubsystem = IntakeSubsystem(intake)
armSubsystem = ArmSubsystem(armLeft, armRight)
elevatorSubsystem = SlidesSubsytem(elevatorLeft, elevatorRight)
armSubsystem = ArmSubsystem(armRight, armLeft)
elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft)

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose)
.turn(Math.toRadians(90.0))
Expand Down
4 changes: 2 additions & 2 deletions ...kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@ class RedHighBasketRight() : OpMode() {

driveSubsystem = DriveSubsystem(hardwareMap)
intakeSubsystem = IntakeSubsystem(intake)
armSubsystem = ArmSubsystem(armLeft, armRight)
elevatorSubsystem = SlidesSubsytem(elevatorLeft, elevatorRight)
armSubsystem = ArmSubsystem(armRight, armLeft)
elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft)

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose)
.turn(Math.toRadians(90.0))
Expand Down
4 changes: 2 additions & 2 deletions ...kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@ class RedHighChamberLeft() : OpMode() {

driveSubsystem = DriveSubsystem(hardwareMap)
intakeSubsystem = IntakeSubsystem(intake)
armSubsystem = ArmSubsystem(armLeft, armRight)
elevatorSubsystem = SlidesSubsytem(elevatorLeft, elevatorRight)
armSubsystem = ArmSubsystem(armRight, armLeft)
elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft)

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose)

Expand Down
4 changes: 2 additions & 2 deletions ...tlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@ class RedHighChamberRight() : OpMode() {

driveSubsystem = DriveSubsystem(hardwareMap)
intakeSubsystem = IntakeSubsystem(intake)
armSubsystem = ArmSubsystem(armLeft, armRight)
elevatorSubsystem = SlidesSubsytem(elevatorLeft, elevatorRight)
armSubsystem = ArmSubsystem(armRight, armLeft)
elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft)

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose)

Expand Down
10 changes: 5 additions & 5 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,16 @@ import kotlin.math.PI

@Config
class ArmSubsystem(
armLeft: Motor,
armRight: Motor
armRight: Motor,
armLeft: Motor
) : PIDSubsystem(
PIDController(
ArmConstants.kP.value,
ArmConstants.kI.value,
ArmConstants.kD.value
)
) {
private val turnMotors = MotorGroup(armLeft, armRight)
private val turnMotors = MotorGroup(armRight, armLeft)

val armAngle: Double
get() = turnMotors.positions[0] / GoBILDA.RPM_60.cpr * PI
Expand All @@ -32,8 +32,8 @@ class ArmSubsystem(
private val feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0);

init {
armLeft.inverted = true
armLeft.encoder.setDirection(Motor.Direction.REVERSE)
armRight.inverted = true
armRight.encoder.setDirection(Motor.Direction.REVERSE)

turnMotors.resetEncoder()
turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
Expand Down
23 changes: 4 additions & 19 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,16 +9,16 @@ import org.firstinspires.ftc.teamcode.utils.PIDSubsystem

@Config
class SlidesSubsytem(
elevatorLeft : Motor,
elevatorRight : Motor
elevatorRight : Motor,
elevatorLeft : Motor
) : PIDSubsystem(
PIDController(
SlidesConstants.kP.value,
SlidesConstants.kI.value,
SlidesConstants.kD.value,
)
) {
private val extendMotors = MotorGroup(elevatorLeft, elevatorRight)
private val extendMotors = MotorGroup(elevatorRight, elevatorLeft)

val slidePos: Double
get() = extendMotors.positions[0]
Expand All @@ -27,30 +27,15 @@ class SlidesSubsytem(
get() = extendMotors.velocities[0]

init {
elevatorLeft.inverted = true
elevatorRight.inverted = true

extendMotors.resetEncoder()
extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
}

override fun useOutput(output: Double, setpoint: Double) {
// For tuning only
controller.setPIDF(kP, kI, kD, 0.0)

extendMotors.set(output)
}

override fun getMeasurement() = slidePos

companion object {

@JvmField
var kP = 0.03

@JvmField
var kI = 0.0

@JvmField
var kD = 0.0004
}
}

0 comments on commit 6b6bad1

Please sign in to comment.