diff --git a/.idea/compiler.xml b/.idea/compiler.xml index fb7f4a8..b589d56 100644 --- a/.idea/compiler.xml +++ b/.idea/compiler.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml index 6199cc2..de4f1ab 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,9 +1,13 @@ - + + + + \ No newline at end of file diff --git a/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/CycleAuto.kt b/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/CycleAuto.kt index 972156f..ebd6512 100644 --- a/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/CycleAuto.kt +++ b/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/CycleAuto.kt @@ -49,7 +49,7 @@ abstract class CycleAuto(val right: Boolean) : CommandLinearOpMode(::Oc val offset = 5.0 val startPose = pose2d(0.0, offset, PI) var intakePose = pose2d(if (right) 52.5 else 52.5, if (right) -21.0 else 20.25, if (right) PI/2.0 else -PI/2.0) - var outputPose = pose2d(if (right) 48.5 else 50.5, if (right) 11.0 else -12.0, if (right) PI else PI) + var outputPose = pose2d(if (right) 48.5 else 48.5, if (right) 11.0 else -12.0, if (right) PI else PI) val MIDDLE = if (right) PI/2.0 else -PI/2.0 val SIDE = if (right) -PI/2.0 else PI/2.0 val cycleOffset = if (right) @@ -101,7 +101,7 @@ abstract class CycleAuto(val right: Boolean) : CommandLinearOpMode(::Oc // VerticalSlides.State.RunTo(VerticalSlides.Height.MID.pos) // } // await { !bot.output.lift.isBusy } - task { bot.output.claw.state = Output.ClawState.OPEN } + task { bot.output.claw.state = Output.ClawState.WIDE } delay(100.milliseconds) task { bot.output.arm.state = Output.ArmState.MAX_UP } delay(200.milliseconds) @@ -208,19 +208,6 @@ abstract class CycleAuto(val right: Boolean) : CommandLinearOpMode(::Oc } } -fun Double.angleWrap(): Double { - var x = this; - while (x >= PI) { - x -= 2 * PI - } - while (x <= -PI) { - x += 2 * PI - } - return x -} - -fun TrajectorySequenceBuilder.turnWrap(angle: Double) = turn(angle.angleWrap()) - @Autonomous(preselectTeleOp = "OctoberTele", group = "_official") class LeftCycleAuto : CycleAuto(false) diff --git a/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/PassthruAuto.kt b/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/PassthruAuto.kt new file mode 100644 index 0000000..c8338a1 --- /dev/null +++ b/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/PassthruAuto.kt @@ -0,0 +1,218 @@ +package org.thenuts.powerplay.opmode.auto + +import com.acmerobotics.roadrunner.geometry.Pose2d +import com.acmerobotics.roadrunner.geometry.Vector2d +import com.qualcomm.robotcore.eventloop.opmode.Autonomous +import org.thenuts.powerplay.acme.trajectorysequence.TrajectorySequenceBuilder +import org.thenuts.powerplay.game.Alliance +import org.thenuts.powerplay.game.Mode +import org.thenuts.powerplay.game.Signal +import org.thenuts.powerplay.opmode.CommandLinearOpMode +import org.thenuts.powerplay.opmode.commands.go +import org.thenuts.powerplay.subsystems.output.VerticalSlides +import org.thenuts.powerplay.subsystems.October +import org.thenuts.powerplay.subsystems.output.Output +import org.thenuts.switchboard.command.Command +import org.thenuts.switchboard.command.CommandScheduler +import org.thenuts.switchboard.dsl.mkSequential +import kotlin.math.PI +import kotlin.time.Duration.Companion.milliseconds + +abstract class PassthruAuto(val right: Boolean) : CommandLinearOpMode(::October, Alliance.RED, Mode.AUTO) { + lateinit var cmd: Command + + override fun postInitHook() { + bot.vision?.front?.startDebug() + bot.vision?.signal?.enable() + bot.vision?.gamepad = gamepad1 + + val reference = Pose2d(if (right) -36.0 else 36.0, 72.0, -PI/2.0) + + fun vec2d(x: Double, y: Double): Vector2d { + if (REFERENCE) + return Vector2d(x, y).rotated(reference.heading) + reference.vec() + else return Vector2d(x, y) + } + + fun pose2d(x: Double, y: Double, heading: Double): Pose2d { + if (REFERENCE) + return Pose2d(vec2d(x, y), heading + reference.heading) + else return Pose2d(x, y, heading) + } + + fun heading(heading: Double): Double { + if (REFERENCE) + return heading + reference.heading + else return heading + } + + val offset = 5.0 + val startPose = pose2d(0.0, offset, PI) + var intakePose = pose2d(if (right) 52.5 else 52.5, if (right) -21.0 else 20.25, if (right) PI/2.0 else -PI/2.0) + var outputPose = pose2d(if (right) 48.5 else 50.5, if (right) 11.0 else -12.0, if (right) PI else PI) + val MIDDLE = if (right) PI/2.0 else -PI/2.0 + val SIDE = if (right) -PI/2.0 else PI/2.0 + val cycleOffset = if (right) + pose2d(0.0, 0.0, 0.0) + else + pose2d(0.0, 2.75, 0.0) + + bot.drive.poseEstimate = startPose + cmd = mkSequential { +// add(ExtendCommand(bot.output, scoreHeight, Output.OutputSide.SAMESIDE)) + task { bot.output.claw.state = Output.ClawState.CLOSED } +// delay(500.milliseconds) + + go(bot.drive, startPose) { + setTangent(-PI/4.0) + splineToConstantHeading(vec2d(3.0, offset/2.0), heading(-PI/4.0)) +// back(3.0) + addDisplacementMarker { + bot.output.arm.state = Output.ArmState.MAX_UP + } +// if (right) { +// strafeRight(24.0 - offset) +// back(48.0) +// } else { +// strafeLeft(24.0 + offset) +// back(48.0) +// } +// strafeLeft(offset) + splineToConstantHeading(vec2d(8.0, 0.0), heading(0.0)) + splineToConstantHeading(vec2d(52.0, 0.0), heading(0.0)) + addDisplacementMarker { + bot.output.lift.state = + VerticalSlides.State.RunTo(VerticalSlides.Height.HIGH.pos) + } + splineToConstantHeading(outputPose.vec(), heading(MIDDLE)) + } + + fun output(height: Int) { + task { + bot.output.lift.state = + VerticalSlides.State.RunTo(height) + } + task { bot.output.arm.state = Output.ArmState.CLEAR } + await { !bot.output.lift.isBusy } +// delay(1000.milliseconds) + task { bot.output.arm.state = Output.ArmState.PASSTHRU_OUTPUT } + delay(300.milliseconds) +// task { +// bot.output.lift.state = +// VerticalSlides.State.RunTo(VerticalSlides.Height.MID.pos) +// } +// await { !bot.output.lift.isBusy } + task { bot.output.claw.state = Output.ClawState.OPEN } + delay(150.milliseconds) + task { bot.output.arm.state = Output.ArmState.CLEAR } + delay(200.milliseconds) + } + + output(VerticalSlides.Height.HIGH.pos) + + repeat(2) { i -> + val stackHeight = 5 - i +// +// go(bot.drive, outputPose) { +// turnWrap(intakePose.heading - outputPose.heading) +// } + + task { bot.output.lift.state = VerticalSlides.State.RunTo(0) } + await { !bot.output.isBusy } + + task { bot.output.arm.state = Output.ArmState.values()[stackHeight - 1] } + await { !bot.output.isBusy } + + go(bot.drive, outputPose/*.copy(heading = intakePose.heading)*/) { + setTangent((outputPose.heading + PI).angleWrap()) + splineTo(intakePose.vec(), heading(SIDE)) + } + + task { bot.output.claw.state = Output.ClawState.CLOSED } + delay(500.milliseconds) + + task { bot.output.lift.state = VerticalSlides.State.RunTo(VerticalSlides.Height.ABOVE_STACK.pos) } + await { !bot.output.lift.isBusy } + task { bot.output.arm.state = Output.ArmState.MAX_UP } + + + go(bot.drive, intakePose) { +// lineToLinearHeading(outputPose) + setTangent(heading(SIDE)) + splineTo(outputPose.vec(), outputPose.heading) + turnWrap(outputPose.heading - intakePose.heading) + } + + intakePose += cycleOffset + outputPose += cycleOffset + + + output(VerticalSlides.Height.HIGH.pos) + } + + switch({ bot.vision!!.signal.finalTarget }) { + value(Signal.LEFT) { + go(bot.drive, outputPose) { + setTangent(heading(SIDE)) + splineToConstantHeading(vec2d(51.0, 0.0), SIDE) + addDisplacementMarker { bot.output.lift.runTo(VerticalSlides.Height.MID.pos) } + splineToConstantHeading(vec2d(32.0, 0.0), PI) + splineToConstantHeading(vec2d(32.0, 23.0), PI/2.0) + splineToConstantHeading(vec2d(35.0, 23.0), 0.0) + } + } + value(Signal.MID) { + go(bot.drive, outputPose) { + setTangent(heading(SIDE)) + splineToConstantHeading(vec2d(51.0, 0.0), SIDE) + addDisplacementMarker { bot.output.lift.runTo(VerticalSlides.Height.MID.pos) } + splineToConstantHeading(vec2d(35.0, 0.0), PI) + } + } + value(Signal.RIGHT) { + go(bot.drive, outputPose) { + setTangent(heading(SIDE)) + splineToConstantHeading(vec2d(51.0, 0.0), SIDE) + addDisplacementMarker { bot.output.lift.runTo(VerticalSlides.Height.MID.pos) } + splineToConstantHeading(vec2d(32.0, 0.0), PI) + splineToConstantHeading(vec2d(32.0, -23.0), -PI/2.0) + splineToConstantHeading(vec2d(35.0, -23.0), 0.0) + } + } + } + task { bot.output.lift.runTo(0) } + delay(1000.milliseconds) + task { stop() } + } + } + + override fun stopHook() { + bot.vision?.front?.stopDebug() + bot.vision?.signal?.disable() + } + + override fun initLoopHook() { + log.out["signal"] = bot.vision?.signal?.finalTarget + } + + override fun postStartHook() { + bot.vision?.front?.stopDebug() + bot.vision?.signal?.disable() + bot.vision?.gamepad = null + sched.addCommand(cmd) + } + + override fun loopHook() { + log.out["COMMAND ORDER"] = sched.nodes.filterIsInstance().map { it.runner.cmd::class.simpleName } + } + + companion object { + @JvmField var REFERENCE = false + } +} + +@Autonomous(preselectTeleOp = "OctoberTele", group = "_official") +class LeftPassthruAuto : PassthruAuto(false) + +@Autonomous(preselectTeleOp = "OctoberTele", group = "_official") +class RightPassthruAuto : PassthruAuto(true) diff --git a/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/ext.kt b/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/ext.kt new file mode 100644 index 0000000..4971cfd --- /dev/null +++ b/TeamCode/src/main/java/org/thenuts/powerplay/opmode/auto/ext.kt @@ -0,0 +1,18 @@ +package org.thenuts.powerplay.opmode.auto + +import org.thenuts.powerplay.acme.trajectorysequence.TrajectorySequenceBuilder +import kotlin.math.PI + + +fun Double.angleWrap(): Double { + var x = this; + while (x > PI) { + x -= 2 * PI + } + while (x <= -PI) { + x += 2 * PI + } + return x +} + +fun TrajectorySequenceBuilder.turnWrap(angle: Double) = turn(angle.angleWrap()) diff --git a/TeamCode/src/main/java/org/thenuts/powerplay/opmode/tele/Driver2.kt b/TeamCode/src/main/java/org/thenuts/powerplay/opmode/tele/Driver2.kt index bfc93cc..87a22df 100644 --- a/TeamCode/src/main/java/org/thenuts/powerplay/opmode/tele/Driver2.kt +++ b/TeamCode/src/main/java/org/thenuts/powerplay/opmode/tele/Driver2.kt @@ -104,7 +104,9 @@ class Driver2(val gamepad: Gamepad, val bot: October, val outputSlot: SlotComman } else if (pad.left_bumper && !prev.left_bumper) { // open claw outputSlot.interrupt(mkSequential { - task { bot.output.claw.state = Output.ClawState.OPEN } + + task { bot.output.claw.state = if (bot.output.claw.state == Output.ClawState.OPEN) Output.ClawState.WIDE + else Output.ClawState.OPEN } await { !bot.output.isBusy } }) /* @@ -189,13 +191,17 @@ class Driver2(val gamepad: Gamepad, val bot: October, val outputSlot: SlotComman task { bot.output.arm.state = Output.ArmState.SAMESIDE_OUTPUT } await { !bot.output.isBusy } }) - } else if (pad.x && false) { -// interruptTo(Output.OutputState.S_LOWER) -// outputSlot.interrupt(mkSequential { -// task { bot.output.arm.state = Output.ArmState.SAMESIDE_OUTPUT } -// await { !bot.output.isBusy } -// }) - if ((!prev.x || frame.n % 4 == 0L) && bot.output.claw.state != Output.ClawState.CLOSED) { + } else if (pad.x && !prev.x) { + interruptTo(Output.OutputState.S_LOWER) + outputSlot.interrupt(mkSequential { + task { + bot.output.arm.state = + if (bot.output.arm.state == Output.ArmState.PASSTHRU_HOVER) Output.ArmState.PASSTHRU_OUTPUT + else Output.ArmState.PASSTHRU_HOVER + } + await { !bot.output.isBusy } + }) + /*if ((!prev.x || frame.n % 4 == 0L) && bot.output.claw.state != Output.ClawState.CLOSED) { val dist = bot.intake_dist.getDistance(DistanceUnit.INCH) bot.log.out["intake_dist"] = dist if (dist < CLAW_DIST) @@ -204,7 +210,7 @@ class Driver2(val gamepad: Gamepad, val bot: October, val outputSlot: SlotComman // task { bot.output.arm.state = Output.ArmState.CLEAR } // await { !bot.output.isBusy } // }) - } + }*/ } else if (pad.y && !prev.y) { // interruptTo(Output.OutputState.CLEAR) outputSlot.interrupt(mkSequential { @@ -214,11 +220,14 @@ class Driver2(val gamepad: Gamepad, val bot: October, val outputSlot: SlotComman } else if (pad.a && !prev.a) { // interruptTo(Output.OutputState.GROUND) outputSlot.interrupt(mkSequential { - task { bot.output.claw.state = Output.ClawState.OPEN } -// task { bot.output.arm.state = Output.ArmState.CLEAR } + if (bot.output.arm.state.pos < Output.ArmState.CLEAR.pos) { + task { bot.output.arm.state = Output.ArmState.CLEAR } + await { !bot.output.isBusy } + } task { bot.output.lift.runTo(0) } await { !bot.output.isBusy } task { bot.output.arm.state = Output.ArmState.values()[intakeHeight - 1] } + task { bot.output.claw.state = Output.ClawState.WIDE } }) } diff --git a/TeamCode/src/main/java/org/thenuts/powerplay/readme.md b/TeamCode/src/main/java/org/thenuts/powerplay/readme.md index 4d1da42..392ebd4 100644 --- a/TeamCode/src/main/java/org/thenuts/powerplay/readme.md +++ b/TeamCode/src/main/java/org/thenuts/powerplay/readme.md @@ -1,4 +1,4 @@ -## TeamCode Module +x## TeamCode Module Welcome! diff --git a/TeamCode/src/main/java/org/thenuts/powerplay/subsystems/output/Output.kt b/TeamCode/src/main/java/org/thenuts/powerplay/subsystems/output/Output.kt index 1f17afd..2b09508 100644 --- a/TeamCode/src/main/java/org/thenuts/powerplay/subsystems/output/Output.kt +++ b/TeamCode/src/main/java/org/thenuts/powerplay/subsystems/output/Output.kt @@ -1,6 +1,8 @@ package org.thenuts.powerplay.subsystems.output import com.acmerobotics.dashboard.config.Config +import com.qualcomm.robotcore.hardware.PwmControl +import com.qualcomm.robotcore.hardware.ServoImplEx import org.thenuts.powerplay.annotations.DiffField import org.thenuts.powerplay.subsystems.util.StatefulServo import org.thenuts.powerplay.subsystems.Subsystem @@ -11,6 +13,7 @@ import org.thenuts.switchboard.core.Logger import org.thenuts.switchboard.dsl.mkSequential import org.thenuts.switchboard.hardware.Configuration import org.thenuts.switchboard.hardware.HardwareOutput +import org.thenuts.switchboard.hardware.ServoImpl import org.thenuts.switchboard.util.Frame import java.util.* import kotlin.math.PI @@ -21,21 +24,22 @@ import kotlin.time.Duration.Companion.seconds @Config class Output(val log: Logger, config: Configuration) : Subsystem { enum class ArmState(override val pos: Double) : StatefulServo.ServoPosition { - INTAKE(0.70), TWO(0.65), THREE(0.60), FOUR(0.55), FIVE(0.50), - PASSTHRU_OUTPUT(0.47), CLEAR(0.22), SAMESIDE_OUTPUT(0.32), - MAX_UP(0.19), - HORIZONTAL(0.47); + INTAKE(1.0), TWO(0.972), THREE(0.944), FOUR(0.916), FIVE(0.888), + PASSTHRU_OUTPUT(0.25), PASSTHRU_HOVER(0.40), CLEAR(0.70), SAMESIDE_HOVER(0.80), SAMESIDE_OUTPUT(0.80 + ), + MAX_UP(0.70), + HORIZONTAL(0.85); fun offset(): Double = cos((pos - HORIZONTAL.pos) * SERVO_RANGE) * ARM_LENGTH companion object { - val SERVO_RANGE = 160.0/180.0 * PI + val SERVO_RANGE = 300.0/180.0 * PI val ARM_LENGTH = 11.12205 } } enum class ClawState(override val pos: Double) : StatefulServo.ServoPosition { - WIDE(0.9), OPEN(0.9), CLOSED(0.63) + WIDE(0.9), OPEN(0.65), CLOSED(0.55), JUNCTION(0.65) } enum class LiftState { @@ -47,7 +51,7 @@ class Output(val log: Logger, config: Configuration) : Subsystem { OutputState.GROUND, OutputState.INTAKE, OutputState.CLEAR, - OutputState.S_OUTPUT, + OutputState.S_HOVER, OutputState.S_LOWER, OutputState.S_DROP ), @@ -56,7 +60,7 @@ class Output(val log: Logger, config: Configuration) : Subsystem { OutputState.GROUND, OutputState.INTAKE, OutputState.CLEAR, - OutputState.P_OUTPUT, + OutputState.P_HOVER, OutputState.P_LOWER, OutputState.P_DROP ); @@ -72,16 +76,19 @@ class Output(val log: Logger, config: Configuration) : Subsystem { GROUND(LiftState.INTAKE, ArmState.INTAKE, ClawState.WIDE), INTAKE(LiftState.INTAKE, ArmState.INTAKE, ClawState.CLOSED), + CLEAR_LOW(LiftState.INTAKE, ArmState.CLEAR, ClawState.CLOSED), + CLEAR(LiftState.CLEAR, ArmState.CLEAR, ClawState.CLOSED), CLEAR_OPEN(LiftState.CLEAR, ArmState.CLEAR, ClawState.OPEN), - S_OUTPUT(LiftState.OUTPUT, ArmState.CLEAR, ClawState.CLOSED), + S_HOVER(LiftState.OUTPUT, ArmState.SAMESIDE_HOVER, ClawState.CLOSED), S_LOWER(LiftState.OUTPUT, ArmState.SAMESIDE_OUTPUT, ClawState.CLOSED), - S_DROP(LiftState.OUTPUT, ArmState.SAMESIDE_OUTPUT, ClawState.OPEN), + S_DROP(LiftState.OUTPUT, ArmState.SAMESIDE_OUTPUT, ClawState.WIDE), - P_OUTPUT(LiftState.OUTPUT, ArmState.CLEAR, ClawState.CLOSED), + P_HOVER(LiftState.OUTPUT, ArmState.PASSTHRU_HOVER, ClawState.CLOSED), P_LOWER(LiftState.OUTPUT, ArmState.PASSTHRU_OUTPUT, ClawState.CLOSED), - P_DROP(LiftState.OUTPUT, ArmState.PASSTHRU_OUTPUT, ClawState.OPEN); + P_DROP(LiftState.OUTPUT, ArmState.PASSTHRU_OUTPUT, ClawState.OPEN), + P_JUNCTION(LiftState.OUTPUT, ArmState.PASSTHRU_OUTPUT, ClawState.JUNCTION); // // BACK_CLOSED(LiftState.INTAKE, ExtensionState.BACK, ClawState.CLOSED), // BACK_INTAKE(LiftState.INTAKE, ExtensionState.BACK, ClawState.OPEN); @@ -101,18 +108,21 @@ class Output(val log: Logger, config: Configuration) : Subsystem { fun legalMoves(): List { return when (this) { GROUND -> listOf(INTAKE) - INTAKE -> listOf(GROUND, CLEAR, S_OUTPUT, P_OUTPUT) + INTAKE -> listOf(GROUND, CLEAR_LOW) + + CLEAR_LOW -> listOf(INTAKE, CLEAR) - CLEAR -> listOf(INTAKE, S_OUTPUT, P_OUTPUT, CLEAR_OPEN) - CLEAR_OPEN -> listOf(CLEAR) + CLEAR -> listOf(CLEAR_LOW, S_HOVER, P_HOVER, CLEAR_OPEN) + CLEAR_OPEN -> listOf(CLEAR, CLEAR_LOW) - S_OUTPUT -> listOf(CLEAR, S_LOWER, INTAKE) - S_LOWER -> listOf(S_OUTPUT, S_DROP, INTAKE) - S_DROP -> listOf(S_LOWER) + S_HOVER -> listOf(CLEAR, S_LOWER, INTAKE) + S_LOWER -> listOf(S_HOVER, S_DROP, INTAKE) + S_DROP -> listOf(CLEAR_OPEN, CLEAR, S_LOWER) - P_OUTPUT -> listOf(CLEAR, P_LOWER, INTAKE) - P_LOWER -> listOf(P_OUTPUT, P_DROP, INTAKE) - P_DROP -> listOf(P_LOWER) + P_HOVER -> listOf(CLEAR, P_LOWER, INTAKE) + P_LOWER -> listOf(P_HOVER, P_DROP, INTAKE) + P_DROP -> listOf(P_LOWER, P_JUNCTION) + P_JUNCTION -> listOf(CLEAR_OPEN) } } @@ -151,8 +161,12 @@ class Output(val log: Logger, config: Configuration) : Subsystem { val lift = VerticalSlides(log, config) val claw = StatefulServo(config.servos["claw_output"], ClawState.WIDE) - val leftArm = config.servos["left_output"] - val rightArm = config.servos["right_output"] + val leftArm = config.servos["left_output"].also { + ((it as? ServoImpl)?.s as? ServoImplEx)?.pwmRange =PwmControl.PwmRange(500.0,2500.0) + } + val rightArm = config.servos["right_output"].also { + ((it as? ServoImpl)?.s as? ServoImplEx)?.pwmRange =PwmControl.PwmRange(500.0,2500.0) + } val linkedServos = LinkedServos(leftArm, rightArm, { LEFT_BACK to RIGHT_BACK } , { LEFT_DOWN to RIGHT_DOWN }).also { it.position = ArmState.INTAKE.pos @@ -228,12 +242,11 @@ class Output(val log: Logger, config: Configuration) : Subsystem { } companion object { - @JvmField var LEFT_BACK = 0.02 + @JvmField var LEFT_BACK = 0.00 @JvmField var LEFT_DOWN = 1.0 - @JvmField var RIGHT_BACK = 0.985 - @JvmField var RIGHT_DOWN = 0.02 - - @JvmField var STEP = 0.05 + @JvmField var RIGHT_BACK = 1.0 + @JvmField var RIGHT_DOWN = 0.00 + @JvmField var STEP = 0.028 @JvmField var MAX_ACCEL = 3.0 @JvmField var MAX_VEL = 10.0 diff --git a/TeamCode/src/main/java/org/thenuts/powerplay/subsystems/output/VerticalSlides.kt b/TeamCode/src/main/java/org/thenuts/powerplay/subsystems/output/VerticalSlides.kt index 9b39c3c..f04eabd 100644 --- a/TeamCode/src/main/java/org/thenuts/powerplay/subsystems/output/VerticalSlides.kt +++ b/TeamCode/src/main/java/org/thenuts/powerplay/subsystems/output/VerticalSlides.kt @@ -56,11 +56,11 @@ class VerticalSlides(val log: Logger, config: Configuration) : Subsystem { val motor1 = config.motors["slides1"].also { it.zpb = Motor.ZeroPowerBehavior.BRAKE - (it as MotorImpl).m.direction = DcMotorSimple.Direction.REVERSE + (it as? MotorImpl)?.m?.direction = DcMotorSimple.Direction.REVERSE } val motor2 = config.motors["slides2"].also { it.zpb = Motor.ZeroPowerBehavior.BRAKE -// (it as MotorImpl).m.direction = DcMotorSimple.Direction.REVERSE +// (it as? MotorImpl)?.m?.direction = DcMotorSimple.Direction.REVERSE } val leftSwitch = config.touchSensors["leftLimit"] @@ -114,7 +114,7 @@ class VerticalSlides(val log: Logger, config: Configuration) : Subsystem { INTAKE(0), MIN_CLEAR(0), ABOVE_STACK(250), TERMINAL(38), GROUND(38), - LOW(0), MID(390), HIGH(765); + LOW(0), MID(335), HIGH(675); } val isBusy: Boolean @@ -227,7 +227,10 @@ class VerticalSlides(val log: Logger, config: Configuration) : Subsystem { if (position < INSIDE_BOT) { if (power > 0.0) power += BOT_FRICTION - else power = max(INSIDE_DROP_POWER, power) + else { + power = max(INSIDE_DROP_POWER, power) + //setZpb(INSIDE_DROP_MODE) + } } setPower(power) } @@ -252,19 +255,20 @@ class VerticalSlides(val log: Logger, config: Configuration) : Subsystem { // @JvmField var MAX_SLIDES_DOWN = 0.1 @JvmField var LIFT_RUN_TO_PID = PIDCoefficients( - kP = 0.005, + kP = 0.01, + kD = 0.00008 ) @JvmField var LIFT_KV = 1.0 @JvmField var LIFT_KA = 0.0 @JvmField var LIFT_KS = 0.0 @JvmField var LIFT_KB = 0.0 - @JvmField var LIFT_KH = 0.0002 + @JvmField var LIFT_KH = 0.00045 @JvmField var TOLERANCE = 200 @JvmField var BRAKE_HEIGHT = 0 - @JvmField var DROP_POWER = -0.5 + @JvmField var DROP_POWER = -0.30 @JvmField var EDGE_POWER = 0.25 @JvmField var MAX_DIFF = 200 @@ -274,7 +278,8 @@ class VerticalSlides(val log: Logger, config: Configuration) : Subsystem { @JvmField var BUSY_THRESHOLD = 150 @JvmField var INSIDE_BOT = 260 - @JvmField var INSIDE_DROP_POWER = -0.25 + @JvmField var INSIDE_DROP_POWER = -0.10 + //@JvmField var INSIDE_DROP_MODE = Motor.ZeroPowerBehavior.BRAKE @JvmField var BOT_FRICTION = 0.1 @JvmField var MAX_ACCEL = 5000.0