Skip to content

Commit

Permalink
drive
Browse files Browse the repository at this point in the history
  • Loading branch information
Ishaan Ghaskadbi authored and Ishaan Ghaskadbi committed Dec 28, 2024
1 parent 0baa50a commit 83881e6
Show file tree
Hide file tree
Showing 3 changed files with 237 additions and 26 deletions.
202 changes: 202 additions & 0 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/Sample1Plus2.kt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,202 @@
package org.firstinspires.ftc.teamcode.opModes.auto

import com.acmerobotics.roadrunner.Pose2d
import com.acmerobotics.roadrunner.Vector2d
import com.arcrobotics.ftclib.command.CommandOpMode
import com.arcrobotics.ftclib.command.InstantCommand
import com.arcrobotics.ftclib.command.ParallelCommandGroup
import com.arcrobotics.ftclib.command.SequentialCommandGroup
import com.arcrobotics.ftclib.command.WaitCommand
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import org.firstinspires.ftc.teamcode.commands.ActionCommand
import org.firstinspires.ftc.teamcode.commands.arm.ArmCommand
import org.firstinspires.ftc.teamcode.commands.elevator.ElevatorCommand
import org.firstinspires.ftc.teamcode.commands.intake.IntakeBeltCommand
import org.firstinspires.ftc.teamcode.commands.intake.ThrowItBackCommand
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem

@Autonomous(name = "Sample 1 + 2")
class Sample1Plus2 : CommandOpMode() {
override fun initialize() {
DriveSubsystem.initialize(hardwareMap)
ArmSubsystem.initialize(hardwareMap)
ElevatorSubsystem.initialize(hardwareMap)
IntakeSubsystem.initialize(hardwareMap)

val scorePreloaded =
SequentialCommandGroup(
InstantCommand({ DriveSubsystem.pose = AutoStartPose.BLUE_LEFT.startPose }),
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder(AutoStartPose.BLUE_LEFT::startPose)
.setTangent(-45.0)
.splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(45.0)) .build(),
DriveSubsystem
),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2500),
InstantCommand({ IntakeSubsystem.closeClaw() }),
),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(2000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(59.0, 59.0))
.build()
),
ThrowItBackCommand(IntakeSubsystem),
InstantCommand({ IntakeSubsystem.openClaw() }),
WaitCommand(500),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(59.0, 59.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(55.0, 55.0))
.build()
),
)


val scoreSample1 =
SequentialCommandGroup(
ParallelCommandGroup(
IntakeBeltCommand(0.0, IntakeSubsystem).withTimeout(500),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
),
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.turn(Math.toRadians(35.0))
.strafeTo(Vector2d(53.0, 52.0))
.build()
),
ElevatorCommand(24.0, ElevatorSubsystem).withTimeout(1000),
),
IntakeBeltCommand(Math.toRadians(-65.0), IntakeSubsystem).withTimeout(700),
WaitCommand(250),
InstantCommand({IntakeSubsystem.closeClaw()}),
WaitCommand(250),
ParallelCommandGroup(
ThrowItBackCommand(IntakeSubsystem).withTimeout(700),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(53.0, 52.0, Math.toRadians(260.0))}
.turn(Math.toRadians(-35.0))
.strafeTo(Vector2d(55.0, 55.0))
.build()
),
),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2500),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(2000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(59.0, 59.0))
.build()
),
WaitCommand(500),
InstantCommand({IntakeSubsystem.openClaw()}),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(59.0, 59.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(55.0, 55.0))
.build()
),
)

val scoreSample2 =
SequentialCommandGroup(
ParallelCommandGroup(
IntakeBeltCommand(0.0, IntakeSubsystem).withTimeout(500),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
),
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.turn(Math.toRadians(51.0))
.strafeTo(Vector2d(56.0, 53.0))
.build()
),
ElevatorCommand(24.0, ElevatorSubsystem).withTimeout(1000),
ThrowItBackCommand(IntakeSubsystem),
),
IntakeBeltCommand(Math.toRadians(-65.0), IntakeSubsystem).withTimeout(700),
WaitCommand(100),
InstantCommand({IntakeSubsystem.closeClaw()}),
WaitCommand(250),
ParallelCommandGroup(
ThrowItBackCommand(IntakeSubsystem).withTimeout(700),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(56.0, 53.0, Math.toRadians(276.0))}
.turn(Math.toRadians(-51.0))
.strafeTo(Vector2d(57.0, 57.0))
.build()
),
),
WaitCommand(500),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2000),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(2000),
WaitCommand(100),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(59.0, 59.0))
.build()
),
WaitCommand(500),
InstantCommand({IntakeSubsystem.openClaw()}),
)





/*val scoreSample3 =
SequentialCommandGroup(
ParallelCommandGroup(
IntakeBeltCommand(0.0, IntakeSubsystem).withTimeout(500),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
),
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(56.0, 56.0, Math.toRadians(225.0))}
.turn(Math.toRadians(70.0))
.build()
),
ElevatorCommand(24.0, ElevatorSubsystem).withTimeout(1000),
ThrowItBackCommand(IntakeSubsystem),
),
IntakeBeltCommand(Math.toRadians(-50.0), IntakeSubsystem).withTimeout(700),
WaitCommand(100),
InstantCommand({IntakeSubsystem.closeClaw()}),
WaitCommand(250),
ParallelCommandGroup(
ThrowItBackCommand(IntakeSubsystem).withTimeout(700),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(56.0, 56.0, Math.toRadians(295.0))}
.turn(Math.toRadians(-70.0))
.build()
),
),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2000),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(1000),
WaitCommand(100),
InstantCommand({IntakeSubsystem.openClaw()}),
WaitCommand(500),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ArmCommand(0.0, ArmSubsystem)
)
*/
schedule(
SequentialCommandGroup(
scorePreloaded,
scoreSample1,
scoreSample2,
// scoreSample3
)
)
}

}
57 changes: 33 additions & 24 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/Sample1Plus3.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,18 +43,18 @@ class Sample1Plus3 : CommandOpMode() {
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(2000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(57.0, 57.0))
.strafeTo(Vector2d(59.0, 59.0))
.build()
),
ThrowItBackCommand(IntakeSubsystem),
InstantCommand({ IntakeSubsystem.openClaw() }),
WaitCommand(500),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(57.0, 57.0, Math.toRadians(225.0))}
DriveSubsystem.actionBuilder{Pose2d(59.0, 59.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(55.0, 55.0))
.build()
),
)
)


val scoreSample1 =
Expand All @@ -66,9 +66,9 @@ class Sample1Plus3 : CommandOpMode() {
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(56.0, 56.0, Math.toRadians(225.0))}
.turn(Math.toRadians(33.0))
.strafeTo(Vector2d(54.0, 53.0))
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.turn(Math.toRadians(35.0))
.strafeTo(Vector2d(53.0, 52.0))
.build()
),
ElevatorCommand(24.0, ElevatorSubsystem).withTimeout(1000),
Expand All @@ -81,29 +81,29 @@ class Sample1Plus3 : CommandOpMode() {
ThrowItBackCommand(IntakeSubsystem).withTimeout(700),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(54.0, 53.0, Math.toRadians(260.0))}
DriveSubsystem.actionBuilder{Pose2d(53.0, 52.0, Math.toRadians(260.0))}
.turn(Math.toRadians(-35.0))
.strafeTo(Vector2d(55.0, 55.0))
.build()
),
),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2500),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(1000),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(2000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(54.0, 53.0, Math.toRadians(260.0))}
.strafeTo(Vector2d(57.0, 57.0))
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(59.0, 59.0))
.build()
),
WaitCommand(100),
WaitCommand(500),
InstantCommand({IntakeSubsystem.openClaw()}),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(57.0, 57.0, Math.toRadians(225.0))}
DriveSubsystem.actionBuilder{Pose2d(59.0, 59.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(55.0, 55.0))
.build()
),
)

/* val scoreSample2 =
val scoreSample2 =
SequentialCommandGroup(
ParallelCommandGroup(
IntakeBeltCommand(0.0, IntakeSubsystem).withTimeout(500),
Expand All @@ -112,8 +112,8 @@ class Sample1Plus3 : CommandOpMode() {
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(56.0, 56.0, Math.toRadians(225.0))}
.turn(Math.toRadians(55.0))
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.turn(Math.toRadians(51.0))
.strafeTo(Vector2d(56.0, 53.0))
.build()
),
Expand All @@ -128,19 +128,28 @@ class Sample1Plus3 : CommandOpMode() {
ThrowItBackCommand(IntakeSubsystem).withTimeout(700),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(57.0, 53.0, Math.toRadians(280.0))}
.turn(Math.toRadians(-55.0))
.strafeTo(Vector2d(55.0, 55.0))
DriveSubsystem.actionBuilder{Pose2d(56.0, 53.0, Math.toRadians(276.0))}
.turn(Math.toRadians(-51.0))
.strafeTo(Vector2d(57.0, 57.0))
.build()
),
),
WaitCommand(500),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2000),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(1000),
IntakeBeltCommand(Math.toRadians(70.0), IntakeSubsystem).withTimeout(500),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(2000),
WaitCommand(100),
InstantCommand({IntakeSubsystem.openClaw()}),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(55.0, 55.0, Math.toRadians(225.0))}
.strafeTo(Vector2d(59.0, 59.0))
.build()
),
WaitCommand(500),
)
InstantCommand({IntakeSubsystem.openClaw()}),
)





val scoreSample3 =
SequentialCommandGroup(
Expand Down Expand Up @@ -179,12 +188,12 @@ class Sample1Plus3 : CommandOpMode() {
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ArmCommand(0.0, ArmSubsystem)
)
*/

schedule(
SequentialCommandGroup(
scorePreloaded,
scoreSample1,
// scoreSample2,
scoreSample2,
// scoreSample3
)
)
Expand Down
4 changes: 2 additions & 2 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,6 +12,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap
import org.firstinspires.ftc.teamcode.constants.ArmConstants
import org.firstinspires.ftc.teamcode.constants.ControlBoard
import kotlin.math.PI
import kotlin.math.cos

@Config
object ArmSubsystem : SubsystemBase() {
Expand Down Expand Up @@ -71,7 +72,7 @@ object ArmSubsystem : SubsystemBase() {
}

fun operateArm() {
val output = controller.calculate(angle) + feedforward.calculate(angle, velocity)
val output = controller.calculate(angle) + ArmConstants.kCos.value * cos(angle)

if (enabled)
turnMotors.set(output)
Expand All @@ -80,7 +81,6 @@ object ArmSubsystem : SubsystemBase() {
// @JvmField var kP = 0.0
// @JvmField var kI = 0.0
// @JvmField var kD = 0.0
//
// @JvmField var kCos = 0.0
// @JvmField var kS = 0.0
// @JvmField var kV = 0.0
Expand Down

0 comments on commit 83881e6

Please sign in to comment.