Skip to content

Commit

Permalink
TeleOp
Browse files Browse the repository at this point in the history
  • Loading branch information
Ishaan Ghaskadbi authored and Ishaan Ghaskadbi committed Nov 29, 2024
1 parent 88a8723 commit 5d310a0
Show file tree
Hide file tree
Showing 12 changed files with 274 additions and 122 deletions.
126 changes: 61 additions & 65 deletions PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,70 +29,66 @@ object BlueAuto {
.addDisplacementMarker {
//add command for rollers servo to outtake
}
.turn(Math.toRadians(30.0)) //
.addTemporalMarker(6.0) {
//add command for arm to go down
//add command for slides to go out
//add command to belt servo to go at [x] angle
}
.addDisplacementMarker {
//add command to intake
}
.turn(Math.toRadians(-30.0)) //
.addTemporalMarker() {
//add command to bring slides in
//add command to bring arm up
//add command to put slides out
//add command to turn belt servo
}
.addDisplacementMarker {
//add command to outtake
}
.turn(Math.toRadians(55.0)) //
.addTemporalMarker() {
//add command to bring slides in
//add command to bring arm down
//add command to put slides out
//add command to turn belt servo
}
.addDisplacementMarker {
//add command to intake
}
.turn(Math.toRadians(-55.0)) //
.addTemporalMarker() {
//add command to bring slides in
//add command to bring arm up
//add command to put slides out
//add command to turn belt servo
}
.addDisplacementMarker {
//add command to outtake
}
.turn(Math.toRadians(80.0)) //
.addTemporalMarker() {
//add command to bring slides in
//add command to bring arm down
//add command to put slides out
//add command to turn belt servo
}
.addDisplacementMarker {
//add command to intake
}
.turn(Math.toRadians(-80.0)) //
.addTemporalMarker() {
//add command to bring slides in
//add command to bring arm up
//add command to put slides out
//add command to turn belt servo
}
.addDisplacementMarker {
//add command to outtake
}
.splineToSplineHeading(Pose2d(36.0, 12.0, Math.toRadians(-180.0)), Math.toRadians(-90.0))
.addDisplacementMarker {
//add command to bring arm up
//add command to bring slides out
}
// .turn(Math.toRadians(30.0)) //
// .addTemporalMarker(6.0) {
// //add command for arm to go down
// //add command for slides to go out
// //add command to belt servo to go at [x] angle
// }
// .addDisplacementMarker {
// //add command to intake
// }
// .turn(Math.toRadians(-30.0)) //
// .addTemporalMarker() {
// //add command to bring slides in
// //add command to bring arm up
// //add command to put slides out
// //add command to turn belt servo
// }
// .addDisplacementMarker {
// //add command to outtake
// }
// .turn(Math.toRadians(55.0)) //
// .addTemporalMarker() {
// //add command to bring slides in
// //add command to bring arm down
// //add command to put slides out
// //add command to turn belt servo
// }
// .addDisplacementMarker {
// //add command to intake
// }
// .turn(Math.toRadians(-55.0)) //
// .addTemporalMarker() {
// //add command to bring slides in
// //add command to bring arm up
// //add command to put slides out
// //add command to turn belt servo
// }
// .addDisplacementMarker {
// //add command to outtake
// }
// .turn(Math.toRadians(80.0)) //
// .addTemporalMarker() {
// //add command to bring slides in
// //add command to bring arm down
// //add command to put slides out
// //add command to turn belt servo
// }
// .addDisplacementMarker {
// //add command to intake
// }
// .turn(Math.toRadians(-80.0)) //
// .addTemporalMarker() {
// //add command to bring slides in
// //add command to bring arm up
// //add command to put slides out
// //add command to turn belt servo
// }
// .addDisplacementMarker {
// //add command to outtake
// }
.lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0)))



Expand All @@ -101,7 +97,7 @@ object BlueAuto {
}
var img: Image? = null
try {
img = ImageIO.read(File("C://Users//arava//Downloads//field.png/"))
img = ImageIO.read(File("/Users/ishaanghaskadbi/Downloads/field.png"))
} catch (_ : IOException) {

}
Expand Down
22 changes: 22 additions & 0 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
package org.firstinspires.ftc.teamcode.commands.intake

import com.arcrobotics.ftclib.command.CommandBase
import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem

class IntakeBeltCommand(
private val intakeBelt: Boolean,
private val intakeBeltSubsystem: IntakeBeltSubsystem
) : CommandBase() {
override fun execute() {
if (intakeBelt) {
intakeBeltSubsystem.intakePos()
}
else {
intakeBeltSubsystem.outtakePos()
}
}

/* override fun end(interrupted: Boolean) {
intakeBeltSubsystem.stop()
}
*/}
4 changes: 2 additions & 2 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
package org.firstinspires.ftc.teamcode.constants

enum class ArmConstants(val value: Double) {
kP(1.0),
kP(2.0),
kI(0.0),
kD(0.08),
kD(0.0),
kCos(0.01)
}
2 changes: 1 addition & 1 deletion TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ enum class ControlBoard(val deviceName: String) {
SLIDES_RIGHT("rightSlideString"),

//Intake
INTAKE("rollerIntake"),
INTAKE("intakeRoller"),
INTAKE_BELT("intakeBelt")


Expand Down
91 changes: 65 additions & 26 deletions ...rc/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,29 +5,34 @@ 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
import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem

@Autonomous(name = "Blue 1 + 3 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp")
@Autonomous(name = "Blue 1 + 1 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp")
class Blue1Plus1: 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 intakeBelt: Servo

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

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

armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
Expand All @@ -36,48 +41,82 @@ class Blue1Plus1: OpMode() {
elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)

driveSubsystem = DriveSubsystem(hardwareMap)
// intakeSubsystem = IntakeSubsystem(intake)
intakeSubsystem = IntakeSubsystem(intake)
intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
armSubsystem = ArmSubsystem(armRight, armLeft)
elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
.addTemporalMarker(0.1) {
armSubsystem.setpoint = Math.toRadians(95.0)
}
.waitSeconds(0.25)
.splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0))
.waitSeconds(0.25)
.back(4.3)
.addTemporalMarker(3.0) {
elevatorSubsystem.setpoint = 1700.0
//turn belt servo
armSubsystem.setpoint = Math.toRadians(97.5)
}
//outtake
.waitSeconds(1.0)
.turn(Math.toRadians(30.0))
.addTemporalMarker(4.0) {
elevatorSubsystem.setpoint = 2000.0
}
.waitSeconds(2.0)
.addTemporalMarker(7.0) {
intakeBeltSubsystem.intakePos()
}
.waitSeconds(1.0)
.addTemporalMarker(8.0) {
intakeSubsystem.outtake()
}
.waitSeconds(0.5)
.addTemporalMarker(8.5) {
intakeSubsystem.stop()
}
.waitSeconds(1.0)
.forward(4.0)
.addTemporalMarker(9.5) {
elevatorSubsystem.setpoint = 0.0
//belt servo turn
}
// .addTemporalMarker(8.0) {
// armSubsystem.setpoint = Math.toRadians(0.0)
// .waitSeconds(2.0)
// .addTemporalMarker(13.0) {
// armSubsystem.setpoint = 0.0
// }
// .waitSeconds(1.0)
// .turn(Math.toRadians(25.0))
// .addTemporalMarker(14.0) {
// elevatorSubsystem.setpoint = 1500.0
// }
// .addTemporalMarker(9.0) {
// elevatorSubsystem.setpoint = 1700.0
// //intake
// .waitSeconds(2.0)
// .addTemporalMarker(16.0) {
// intakeBeltSubsystem.intakePos()
// }
// .waitSeconds(1.0)
// .addTemporalMarker(17.0) {
// intakeSubsystem.intake()
// }
// .addTemporalMarker(12.0) {
// elevatorSubsystem.setpoint = 0.0
// .waitSeconds(2.0)
// .addTemporalMarker(20.0) {
// elevatorSubsystem.setpoint = 100.0
// }
// .addTemporalMarker(15.0) {
// .waitSeconds(2.0)
// .turn(Math.toRadians(-25.0))
// .addTemporalMarker(24.0) {
// armSubsystem.setpoint = 95.0
// //turn belt servo
// intakeBeltSubsystem.outtakePos()
//
// }
// .addTemporalMarker(18.0) {
// //outtake
// .addTemporalMarker(25.0) {
// elevatorSubsystem.setpoint = 2000.0
// }
// .waitSeconds(2.0)
// .splineToSplineHeading(Pose2d(36.0, 12.0, Math.toRadians(-180.0)), Math.toRadians(-90.0))
// .turn(Math.toRadians(-30.0))
// .addTemporalMarker(28.0) {
// intakeSubsystem.outtake()
// }
// .waitSeconds(2.0)
.lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0)))
.addTemporalMarker(12.0) {
armSubsystem.setpoint = 50.0
}
.waitSeconds(1.0)
.addTemporalMarker(13.0) {
elevatorSubsystem.setpoint = 1000.0
}
.build()

driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
Expand Down
Loading

0 comments on commit 5d310a0

Please sign in to comment.