Skip to content

Commit

Permalink
sachet
Browse files Browse the repository at this point in the history
  • Loading branch information
Ishaan Ghaskadbi authored and Ishaan Ghaskadbi committed Nov 27, 2024
1 parent 3b3a0c7 commit 0c350a5
Show file tree
Hide file tree
Showing 10 changed files with 15 additions and 31 deletions.
4 changes: 2 additions & 2 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 @@ -43,11 +43,11 @@ class BlueHighBasketLeft() : OpMode() {
.turn(Math.toRadians(90.0))
.forward(60.0)
.addTemporalMarker(3.0) {
armSubsystem.setpoint = Math.toRadians(105.0)
armSubsystem.setpoint = Math.toRadians(95.0)
}
.waitSeconds(1.0)
.addTemporalMarker(6.0) {
elevatorSubsystem.setpoint = 30.0
elevatorSubsystem.setpoint = 250.0
}
.waitSeconds(2.0)
//.addTemporalMarker(5.0){
Expand Down
2 changes: 1 addition & 1 deletion ...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,7 +38,7 @@ class BlueHighBasketRight() : OpMode() {

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

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose)
Expand Down
2 changes: 1 addition & 1 deletion ...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,7 +36,7 @@ class BlueHighChamberLeft() : OpMode() {

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

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
Expand Down
2 changes: 1 addition & 1 deletion ...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,7 +36,7 @@ class BlueHighChamberRight() : OpMode() {

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

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose)
Expand Down
2 changes: 1 addition & 1 deletion ...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,7 +37,7 @@ class RedHighBasketLeft() : OpMode() {

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

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose)
Expand Down
2 changes: 1 addition & 1 deletion ...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,7 +36,7 @@ class RedHighBasketRight() : OpMode() {

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

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose)
Expand Down
2 changes: 1 addition & 1 deletion ...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,7 +36,7 @@ class RedHighChamberLeft() : OpMode() {

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

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose)
Expand Down
2 changes: 1 addition & 1 deletion ...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,7 +36,7 @@ class RedHighChamberRight() : OpMode() {

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

val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose)
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ class ArmPIDTuner : CommandOpMode() {
armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_312)
armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_312)

armSubsystem = ArmSubsystem(armRight, armLeft, telemetry)
armSubsystem = ArmSubsystem(armRight, armLeft)

RunCommand({
armSubsystem.setpoint = Math.toRadians(target)
Expand Down
26 changes: 5 additions & 21 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 @@ -6,6 +6,7 @@ import com.arcrobotics.ftclib.controller.wpilibcontroller.ArmFeedforward
import com.arcrobotics.ftclib.hardware.motors.Motor
import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA
import com.arcrobotics.ftclib.hardware.motors.MotorGroup
import org.firstinspires.ftc.teamcode.constants.ArmConstants
import org.firstinspires.ftc.teamcode.utils.PIDSubsystem
import kotlin.math.PI

Expand All @@ -15,9 +16,9 @@ class ArmSubsystem(
armRight: Motor
) : PIDSubsystem(
PIDController(
0.0,
0.0,
0.0,
ArmConstants.kP.value,
ArmConstants.kI.value,
ArmConstants.kD.value
)
) {
private val turnMotors = MotorGroup(armLeft, armRight)
Expand All @@ -28,7 +29,7 @@ class ArmSubsystem(
val armVelocity: Double
get() = turnMotors.velocities[0] / GoBILDA.RPM_60.cpr * PI

private val feedforward = ArmFeedforward(0.0, kCos, 0.0);
private val feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0);

init {
armLeft.inverted = true
Expand All @@ -39,25 +40,8 @@ class ArmSubsystem(
}

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

turnMotors.set(output + feedforward.calculate(armAngle, armVelocity))
}

override fun getMeasurement() = armAngle

companion object {
@JvmField
var kCos = 0.004

@JvmField
var kP = 1.0

@JvmField
var kI = 0.0001

@JvmField
var kD = 0.08
}
}

0 comments on commit 0c350a5

Please sign in to comment.