diff --git a/864-robot-config.json b/864-robot-config.json index a2822ae..b4390c0 100644 --- a/864-robot-config.json +++ b/864-robot-config.json @@ -7,10 +7,10 @@ }, "drivetrain": { "ports": { - "rightBack": 13, - "rightFront": 12, "leftBack": 11, - "leftFront": 10 + "leftFront": 10, + "rightBack": 13, + "rightFront": 12 }, "properties": { "maxLeftVelocity": [ @@ -37,7 +37,7 @@ "turnControlGains": { "kp": { "num": [ - 50, + 60, "Percent$" ], "den": [ @@ -69,7 +69,7 @@ "forwardPositionControlGains": { "kp": { "num": [ - 100, + 50, "Percent$" ], "den": [ @@ -101,7 +101,7 @@ "turnPositionControlGains": { "kp": { "num": [ - 100, + 85, "Percent$" ], "den": [ @@ -133,7 +133,7 @@ "leftControlGains": { "kp": { "num": [ - 30, + 50, "Percent$" ], "den": [ @@ -165,7 +165,7 @@ "rightControlGains": { "kp": { "num": [ - 30, + 50, "Percent$" ], "den": [ @@ -204,26 +204,16 @@ ] } }, - "agitator": { - "ports": { - "motor": 2 - }, - "properties": { - "spinSpeed": [ - -50, - "Percent$" - ] - } - }, + "agitator": null, "camSelect": { "port": { "leftCamPort": 5804, "rightCamPort": 5805, - "driveCamPort": 5803 + "driveCamPort": 5811 }, "properties": { - "coprocessorHostname": "10.8.46.19", - "mjpegPath": "/stream.mjpg" + "coprocessorHostname": "10.8.64.20", + "mjpegPath": "/?action=stream" } }, "climberPuller": { @@ -238,176 +228,38 @@ ] } }, - "collectorElevator": { - "port": { - "motor": 1 - }, - "properties": { - "collectSpeed": [ - 100, - "Percent$" - ] - } - }, - "collectorExtender": { - "port": { - "pneumatic": 3 - } - }, - "collectorRollers": { + "collectorElevator": null, + "collectorExtender": null, + "collectorRollers": null, + "gearRoller": { "ports": { - "rollerChannel": 0 + "motor": 16 }, - "properties": { - "lowRollerSpeedOutput": [ - 0, + "props": { + "defaultHoldingPower": [ + 10, "Percent$" ], - "highRollerSpeedOutput": [ + "intakeGearPower": [ 100, "Percent$" - ] - } - }, - "gearGrabber": { - "port": { - "pneumatic": 1, - "proximitySensor": 0 - }, - "props": { - "detectingDistance": [ - 2.2, - "Volts$" - ] - } - }, - "gearTilter": { - "port": { - "pneumatic": 0 - } - }, - "shooterFlywheel": { - "ports": { - "leftMotor": 3, - "rightMotor": 4, - "leftHall": 0, - "rightHall": 1 - }, - "props": { - "maxVelocityLeft": [ - 6500, - "RevolutionsPerMinute$" - ], - "maxVelocityRight": [ - 6500, - "RevolutionsPerMinute$" ], - "velocityGainsLeft": { - "kp": { - "num": [ - 100, - "Percent$" - ], - "den": [ - 3000, - "RevolutionsPerMinute$" - ] - }, - "ki": { - "num": [ - 0, - "Percent$" - ], - "den": [ - 1000, - "Each$" - ] - }, - "kd": { - "num": [ - 0, - "Percent$" - ], - "den": [ - 1000, - "RevolutionsPerMinute$ / s" - ] - } - }, - "velocityGainsRight": { - "kp": { - "num": [ - 100, - "Percent$" - ], - "den": [ - 3000, - "RevolutionsPerMinute$" - ] - }, - "ki": { - "num": [ - 0, - "Percent$" - ], - "den": [ - 1000, - "Each$" - ] - }, - "kd": { - "num": [ - 0, - "Percent$" - ], - "den": [ - 1000, - "RevolutionsPerMinute$ / s" - ] - } - }, - "lowShootSpeedLeft": [ - 3650, - "RevolutionsPerMinute$" - ], - "lowShootSpeedRight": [ - 3650, - "RevolutionsPerMinute$" - ], - "midShootSpeedLeft": [ - 3700, - "RevolutionsPerMinute$" - ], - "midShootSpeedRight": [ - 3700, - "RevolutionsPerMinute$" - ], - "fastShootSpeedLeft": [ - 3750, - "RevolutionsPerMinute$" - ], - "fastShootSpeedRight": [ - 3750, - "RevolutionsPerMinute$" - ], - "currentLimit": [ - 50, + "emitGearPower": [ + -50, "Percent$" ], - "speedTolerance": [ - 75, - "RevolutionsPerMinute$" + "gearDetectionCurrent": [ + 10, + "Amperes$" ] } }, - "shooterShifter": { - "ports": { - "pneumatic": 2 - } - }, - "loadTray": { + "gearTilter": { "port": { - "pneumatic": 4 + "pneumatic": 0 } - } + }, + "shooterFlywheel": null, + "shooterShifter": null, + "loadTray": null } diff --git a/build.sbt b/build.sbt index d8a039a..1ad05eb 100644 --- a/build.sbt +++ b/build.sbt @@ -1,7 +1,7 @@ enablePlugins(FRCPlugin) organization := "com.lynbrookrobotics" -teamNumber := 846 +teamNumber := 864 scalaVersion := "2.12.3" diff --git a/project/Versions.scala b/project/Versions.scala index 9c6f249..7aba6fe 100644 --- a/project/Versions.scala +++ b/project/Versions.scala @@ -1,3 +1,3 @@ object Versions { - val potassiumVersion = "0.1.0-feacaab8" + val potassiumVersion = "0.1.0-2481b217" } \ No newline at end of file diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala b/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala index e55ba7a..b21cbba 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala @@ -9,7 +9,8 @@ import com.lynbrookrobotics.seventeen.collector.extender.CollectorExtender import com.lynbrookrobotics.seventeen.collector.rollers.CollectorRollers import com.lynbrookrobotics.seventeen.drivetrain.Drivetrain import com.lynbrookrobotics.seventeen.drivetrain.unicycleTasks._ -import com.lynbrookrobotics.seventeen.gear.grabber.{GearGrabber, OpenGrabber} +import com.lynbrookrobotics.seventeen.gear.GearTasks +import com.lynbrookrobotics.seventeen.gear.roller.GearRoller import com.lynbrookrobotics.seventeen.gear.tilter.{ExtendTilter, GearTilter} import com.lynbrookrobotics.seventeen.loadtray.LoadTray import com.lynbrookrobotics.seventeen.shooter.ShooterTasks @@ -26,10 +27,7 @@ class AutoGenerator(r: CoreRobot) { private val gearPegDistance = Inches(109) - private val midShootSpeedLeft = r.coreTicks.map(_ => shooterFlywheelProps.get.midShootSpeedLeft) - private val midShootSpeedRight = r.coreTicks.map(_ => shooterFlywheelProps.get.midShootSpeedRight) - - def slowCrossLine(drivetrain: Drivetrain): FiniteTask = { + def slowCrossLine(drivetrain: Drivetrain): FiniteTask = { new DriveDistanceStraight( Inches(107) - robotLength, Inches(3), @@ -38,54 +36,64 @@ class AutoGenerator(r: CoreRobot) { )(drivetrain).withTimeout(Seconds(8)) } - def toGearAndDrop(drivetrain: Drivetrain, gearGrabber: GearGrabber, gearTilter: GearTilter): FiniteTask = { + def toGearAndDrop(drivetrain: Drivetrain, gearRoller: GearRoller, gearTilter: GearTilter): FiniteTask = { val dropAndBack = new WaitTask(Seconds(1)).then(new DriveDistanceStraight( -Feet(2), Inches(3), Degrees(10), Percent(30) - )(drivetrain)).withTimeout(Seconds(5)).andUntilDone( - new OpenGrabber(gearGrabber) and new ExtendTilter(gearTilter) + )(drivetrain)).withTimeout(Seconds(2)).andUntilDone( + GearTasks.scoreGear(gearTilter, gearRoller) ) dropAndBack } - def centerGear(drivetrain: Drivetrain, gearGrabber: GearGrabber, gearTilter: GearTilter): FiniteTask = { + def centerGear(drivetrain: Drivetrain, gearRoller: GearRoller, gearTilter: GearTilter): FiniteTask = { new DriveDistanceStraight( - gearPegDistance - robotLength, + gearPegDistance - robotLength + Inches(10.5 / 2) /* gear peg */ + Inches(3.5) /* bumper */, Inches(3), Degrees(10), - Percent(30) + Percent(25), + minStableTicks = 1 )(drivetrain).withTimeout(Seconds(8)).then( - toGearAndDrop(drivetrain, gearGrabber, gearTilter) + toGearAndDrop(drivetrain, gearRoller, gearTilter) ) } - def rightGear(drivetrain: Drivetrain, gearGrabber: GearGrabber, gearTilter: GearTilter): FiniteTask = { + def rightGear(drivetrain: Drivetrain, gearRoller: GearRoller, gearTilter: GearTilter): FiniteTask = { new DriveDistanceStraight( - Inches(90.5), - Inches(3), + Inches(91.8 - 4 /* going too long in tests */), + Inches(1), + Degrees(5), + Percent(30) + )(drivetrain).withTimeout(Seconds(3)).then(new RotateByAngle( + Degrees(-58), + Degrees(1), + 20 + )(drivetrain).withTimeout(Seconds(2))).then(new DriveDistanceStraight( + Inches(19.75 + 3.5 /* for bumpers */ + 6 /* go in more */), + Inches(2), Degrees(10), Percent(30) - )(drivetrain).withTimeout(Seconds(8)).then(new RotateByAngle( - Degrees(-57.61), - Degrees(5), - 5 - )(drivetrain).withTimeout(Seconds(5))).then(new DriveDistanceStraight( - Inches(19.75), - Inches(3), + )(drivetrain).withTimeout(Seconds(2))).then( + toGearAndDrop(drivetrain, gearRoller, gearTilter) + ).then(new RotateByAngle( + Degrees(58), + Degrees(1), + 20 + )(drivetrain).withTimeout(Seconds(2))).then(new DriveDistanceStraight( + Feet(3), + Inches(2), Degrees(10), Percent(30) - )(drivetrain).withTimeout(Seconds(5))).then( - toGearAndDrop(drivetrain, gearGrabber, gearTilter) - ) + )(drivetrain).withTimeout(Seconds(3))) } - def leftGear(drivetrain: Drivetrain, gearGrabber: GearGrabber, gearTilter: GearTilter): FiniteTask = { + def leftGear(drivetrain: Drivetrain, gearRoller: GearRoller, gearTilter: GearTilter): FiniteTask = { new DriveDistanceStraight( Inches(90.5), - Inches(3), + Inches(1), Degrees(10), Percent(30) )(drivetrain).withTimeout(Seconds(8)).then(new RotateByAngle( @@ -98,12 +106,12 @@ class AutoGenerator(r: CoreRobot) { Degrees(10), Percent(30) )(drivetrain).withTimeout(Seconds(5))).then( - toGearAndDrop(drivetrain, gearGrabber, gearTilter) + toGearAndDrop(drivetrain, gearRoller, gearTilter) ) } def leftGearPurePursuit(drivetrain: Drivetrain, - gearGrabber: GearGrabber, + gearRoller: GearRoller, gearTilter: GearTilter): FiniteTask = { val relativeTurn = drivetrainHardware.turnPosition.relativize((init, curr) => { curr - init @@ -130,12 +138,12 @@ class AutoGenerator(r: CoreRobot) { turnPosition = relativeTurn, steadyOutput = Percent(50) )(drivetrain).withTimeout(Seconds(5)).then( - toGearAndDrop(drivetrain, gearGrabber, gearTilter) + toGearAndDrop(drivetrain, gearRoller, gearTilter) ) } def rightGearPurePursuit(drivetrain: Drivetrain, - gearGrabber: GearGrabber, - gearTilter: GearTilter): FiniteTask = { + gearRoller: GearRoller, + gearTilter: GearTilter): FiniteTask = { val relativeTurn = drivetrainHardware.turnPosition.relativize((init, curr) => { curr - init }) @@ -161,7 +169,7 @@ class AutoGenerator(r: CoreRobot) { turnPosition = relativeTurn, steadyOutput = Percent(50) )(drivetrain).withTimeout(Seconds(5)).then( - toGearAndDrop(drivetrain, gearGrabber, gearTilter) + toGearAndDrop(drivetrain, gearRoller, gearTilter) ) } @@ -202,10 +210,10 @@ class AutoGenerator(r: CoreRobot) { collectorExtender: CollectorExtender, loadTray: LoadTray): FiniteTask = { val shooting = ShooterTasks.continuousShoot( - midShootSpeedLeft, - midShootSpeedRight + shooterFlywheel.midShootSpeedLeft, + shooterFlywheel.midShootSpeedRight )(collectorElevator, collectorRollers, agitator, shooterFlywheel, collectorExtender, loadTray).and( - new ShiftShooter(midShootSpeedLeft.mapToConstant(ShooterShiftLeft))(shooterShifter) + new ShiftShooter(shooterFlywheel.midShootSpeedLeft.mapToConstant(ShooterShiftLeft))(shooterShifter) ) new WaitTask(Seconds(5)).andUntilDone(shooting).then(new DriveDistanceStraight( @@ -215,6 +223,7 @@ class AutoGenerator(r: CoreRobot) { Percent(30) )(drivetrain).withTimeout(Seconds(3))) } + def leftHopperAndShoot(drivetrain: Drivetrain, collectorElevator: CollectorElevator, collectorRollers: CollectorRollers, @@ -225,10 +234,10 @@ class AutoGenerator(r: CoreRobot) { loadTray: LoadTray): ContinuousTask = { val shooting = ShooterTasks.continuousShoot( - midShootSpeedLeft, - midShootSpeedRight + shooterFlywheel.midShootSpeedLeft, + shooterFlywheel.midShootSpeedRight )(collectorElevator, collectorRollers, agitator, shooterFlywheel, collectorExtender, loadTray).and( - new ShiftShooter(midShootSpeedLeft.mapToConstant(ShooterShiftLeft))(shooterShifter) + new ShiftShooter(shooterFlywheel.midShootSpeedLeft.mapToConstant(ShooterShiftLeft))(shooterShifter) ) hopperForward(drivetrain).then(new RotateByAngle( @@ -249,10 +258,10 @@ class AutoGenerator(r: CoreRobot) { collectorExtender: CollectorExtender, loadTray: LoadTray): ContinuousTask = { val shooting = ShooterTasks.continuousShoot( - midShootSpeedLeft, - midShootSpeedRight + shooterFlywheel.midShootSpeedLeft, + shooterFlywheel.midShootSpeedRight )(collectorElevator, collectorRollers, agitator, shooterFlywheel, collectorExtender, loadTray).and( - new ShiftShooter(midShootSpeedLeft.mapToConstant(ShooterShiftRight))(shooterShifter) + new ShiftShooter(shooterFlywheel.midShootSpeedLeft.mapToConstant(ShooterShiftRight))(shooterShifter) ) hopperForward(drivetrain).then(new RotateByAngle( @@ -284,7 +293,7 @@ class AutoGenerator(r: CoreRobot) { ).toContinuous } - def centerGearAndCrossLine(drivetrain: Drivetrain, gearGrabber: GearGrabber, gearTilter: GearTilter): FiniteTask = { + def centerGearAndCrossLine(drivetrain: Drivetrain, gearRoller: GearRoller, gearTilter: GearTilter): FiniteTask = { val relativeTurn = drivetrainHardware.turnPosition.relativize((init, curr) => { curr - init }) @@ -301,7 +310,7 @@ class AutoGenerator(r: CoreRobot) { Degrees(10), Percent(30) )(drivetrain).withTimeout(Seconds(8)).then( - toGearAndDrop(drivetrain, gearGrabber, gearTilter) + toGearAndDrop(drivetrain, gearRoller, gearTilter) ).then(new FollowWayPointsWithPosition( Seq( new Point( @@ -333,7 +342,7 @@ class AutoGenerator(r: CoreRobot) { } def shootCenterGear(drivetrain: Drivetrain, - gearGrabber: GearGrabber, + gearRoller: GearRoller, gearTilter: GearTilter, collectorElevator: CollectorElevator, collectorRollers: CollectorRollers, @@ -343,9 +352,9 @@ class AutoGenerator(r: CoreRobot) { loadTray: LoadTray): FiniteTask = { new WaitTask(Seconds(3)).andUntilDone( ShooterTasks.continuousShoot( - midShootSpeedLeft, - midShootSpeedRight + shooterFlywheel.midShootSpeedLeft, + shooterFlywheel.midShootSpeedRight )(collectorElevator, collectorRollers, agitator, shooterFlywheel, collectorExtender, loadTray) - ).then(centerGear(drivetrain, gearGrabber, gearTilter)) + ).then(centerGear(drivetrain, gearRoller, gearTilter)) } } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala b/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala index 1d6722e..56a40e2 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala @@ -8,7 +8,8 @@ import com.lynbrookrobotics.seventeen.camselect._ import com.lynbrookrobotics.seventeen.climber.puller.RunPuller import com.lynbrookrobotics.seventeen.collector.CollectorTasks import com.lynbrookrobotics.seventeen.collector.extender.CollectorExtenderExtended -import com.lynbrookrobotics.seventeen.gear.grabber.OpenGrabber +import com.lynbrookrobotics.seventeen.gear.GearTasks +import com.lynbrookrobotics.seventeen.gear.roller.RollInwards import com.lynbrookrobotics.seventeen.gear.tilter.ExtendTilter import com.lynbrookrobotics.seventeen.loadtray.ExtendTray import com.lynbrookrobotics.seventeen.shooter.ShooterTasks @@ -18,7 +19,6 @@ import squants.Percent import squants.time._ class ButtonMappings(r: CoreRobot) { - import r._ var curFlywheelTargetLeft: Frequency = if (config.get.shooterFlywheel != null) { @@ -138,37 +138,34 @@ class ButtonMappings(r: CoreRobot) { } for { - gearGrabber <- gearGrabber + gearRoller <- gearRoller gearTilter <- gearTilter } { - val bothPressed = Signal(driverHardware.operatorJoystick.getRawButton(JoystickButtons.RightFour) - && driverHardware.operatorJoystick.getRawButton(JoystickButtons.RightFive)).filter(identity) - - val onlyRightFourPressed = Signal(driverHardware.operatorJoystick.getRawButton(JoystickButtons.RightFour) - && !driverHardware.operatorJoystick.getRawButton(JoystickButtons.RightFive)).filter(identity) - - val onlyRightFivePressed = Signal(!driverHardware.operatorJoystick.getRawButton(JoystickButtons.RightFour) - && driverHardware.operatorJoystick.getRawButton(JoystickButtons.RightFive)).filter(identity) + val rightFourPressed = driverHardware.operatorJoystick.buttonPressed(JoystickButtons.RightFour) + val rightFivePressed = driverHardware.operatorJoystick.buttonPressed(JoystickButtons.RightFive) + val rightSixPressed = driverHardware.operatorJoystick.buttonPressed(JoystickButtons.RightSix) /** - * Releases gear + * Collects gear * only RightFour pressed */ - onlyRightFourPressed.foreach(new OpenGrabber(gearGrabber)) + rightFourPressed.foreach(new RollInwards(gearRoller) and new ExtendTilter(gearTilter)) /** * Extends tilter * only RightFive pressed */ - onlyRightFivePressed.foreach(new ExtendTilter(gearTilter)) + rightFivePressed.foreach(GearTasks.scoreGear(gearTilter, gearRoller)) - /** - * Extends tilter and opens grabber - * both RightFour and RightFive pressed - */ - bothPressed.foreach( - new OpenGrabber(gearGrabber).and(new ExtendTilter(gearTilter)) - ) + rightSixPressed.onStart.foreach(() => { + println("DISABLING AUTO RUN") + gearRoller.disabledAutoRun = true + }) + + if (collectorRollers.isEmpty) { + driverHardware.driverJoystick.buttonPressed(JoystickButtons.Trigger) + .foreach(GearTasks.collectGear(gearTilter, gearRoller).toContinuous) + } } for { diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala b/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala index 2d7ceb9..1cfe861 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala @@ -13,7 +13,7 @@ import com.lynbrookrobotics.seventeen.collector.elevator.CollectorElevator import com.lynbrookrobotics.seventeen.collector.extender.CollectorExtender import com.lynbrookrobotics.seventeen.collector.rollers.CollectorRollers import com.lynbrookrobotics.seventeen.drivetrain._ -import com.lynbrookrobotics.seventeen.gear.grabber.GearGrabber +import com.lynbrookrobotics.seventeen.gear.roller.GearRoller import com.lynbrookrobotics.seventeen.gear.tilter.GearTilter import com.lynbrookrobotics.seventeen.lighting.{SerialComms, StatusLightingComponent} import com.lynbrookrobotics.seventeen.loadtray.LoadTray @@ -77,20 +77,23 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni if (config.get.collectorRollers != null) Some(new CollectorRollers(coreTicks)) else None // Gear Grabber - implicit val gearGrabberHardware = hardware.gearGrabber - implicit val gearGrabberProps = config.map(_.gearGrabber.props) - val gearGrabber: Option[GearGrabber] = { - implicit val gt = () => gearTilter - if (config.get.gearGrabber != null) Some(new GearGrabber(coreTicks)) else None + implicit val gearRollerHardware = hardware.gearRoller + implicit val gearGrabberProps = config.map(_.gearRoller.props) + val gearRoller: Option[GearRoller] = { + if (config.get.gearRoller != null) Some(new GearRoller(coreTicks)) else None } + println(gearRoller) + // Gear Tilter implicit val gearTilterHardware = hardware.gearTilter val gearTilter: Option[GearTilter] = if (config.get.gearTilter != null) { - Some(new GearTilter(coreTicks, gearGrabber, collectorExtender)) + Some(new GearTilter(coreTicks, collectorExtender)) } else None + println(gearTilter) + // Shooter Flywheel implicit val shooterFlywheelHardware = hardware.shooterFlywheel implicit val shooterFlywheelProps = config.map(_.shooterFlywheel.props) @@ -114,7 +117,7 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni * Function to determine what lighting effect should be displayed */ val lightingStatus: () => Int = () => { - val gearState = gearGrabber.isDefined && gearGrabberHardware.proximitySensor.getVoltage > gearGrabberProps.get.detectingDistance.value + val gearState = gearRoller.isDefined && false if (climberPuller.isDefined && climberPullerHardware.motorA.get() > 0) { 9 } else if (gearState) { @@ -148,7 +151,7 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni collectorElevator, collectorExtender, collectorRollers, - gearGrabber, + gearRoller, gearTilter, shooterFlywheel, shooterShifter, @@ -181,37 +184,37 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni for { drivetrain <- drivetrain - gearGrabber <- gearGrabber + gearRoller <- gearRoller gearTilter <- gearTilter } { addAutonomousRoutine(1)( - generator.centerGear(drivetrain, gearGrabber, gearTilter).toContinuous + generator.centerGear(drivetrain, gearRoller, gearTilter).toContinuous ) addAutonomousRoutine(2)( - generator.leftGear(drivetrain, gearGrabber, gearTilter).toContinuous + generator.leftGear(drivetrain, gearRoller, gearTilter).toContinuous ) addAutonomousRoutine(3)( - generator.rightGear(drivetrain, gearGrabber, gearTilter).toContinuous + generator.rightGear(drivetrain, gearRoller, gearTilter).toContinuous ) addAutonomousRoutine(4)( - generator.centerGearAndCrossLine(drivetrain, gearGrabber, gearTilter).toContinuous + generator.centerGearAndCrossLine(drivetrain, gearRoller, gearTilter).toContinuous ) addAutonomousRoutine(11) { - generator.leftGearPurePursuit(drivetrain, gearGrabber, gearTilter).toContinuous + generator.leftGearPurePursuit(drivetrain, gearRoller, gearTilter).toContinuous } addAutonomousRoutine(12) { - generator.rightGearPurePursuit(drivetrain, gearGrabber, gearTilter).toContinuous + generator.rightGearPurePursuit(drivetrain, gearRoller, gearTilter).toContinuous } } for { drivetrain <- drivetrain - gearGrabber <- gearGrabber + gearGrabber <- gearRoller gearTilter <- gearTilter collectorElevator <- collectorElevator collectorRollers <- collectorRollers @@ -301,7 +304,7 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni } val dashboard = Future { - val dashboard = new FunkyDashboard(100, 8080) + val dashboard = new FunkyDashboard(10, 8080) dashboard.start() dashboard } @@ -312,10 +315,6 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni import CoreRobot.ToTimeSeriesNumeric println("Funky Dashboard is up!") - Runtime.getRuntime.addShutdownHook(new Thread(() => { - println("Shutting down Funky Dashboard") - board.stop() - })) board.datasetGroup("Config").addDataset(new JsonEditor("Robot Config")( configFileValue.get, @@ -382,9 +381,9 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni )) } - gearGrabber.foreach { g => - board.datasetGroup("Grabber").addDataset(new TimeSeriesNumeric("IR Distance")( - hardware.gearGrabber.proximitySensor.getVoltage + gearRoller.foreach { g => + board.datasetGroup("Grabber").addDataset(new TimeSeriesNumeric("Motor Current")( + hardware.gearRoller.motor.getOutputCurrent )) } } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala b/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala index c5bd325..23b487a 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala @@ -10,18 +10,18 @@ object DefaultConfig { | }, | "drivetrain": { | "ports": { - | "rightBack": 11, + | "rightBack": 13, | "rightFront": 12, - | "leftBack": 14 , - | "leftFront": 13 + | "leftBack": 11, + | "leftFront": 10 | }, | "properties": { | "maxLeftVelocity": [ - | 15, + | 21.9, | "FeetPerSecond$" | ], | "maxRightVelocity": [ - | 15, + | 23.1, | "FeetPerSecond$" | ], | "maxAcceleration": [ @@ -36,15 +36,15 @@ object DefaultConfig { | 21.75, | "Inches$" | ], - | "gearRatio": 0.3125, + | "gearRatio": 0.5, | "turnControlGains": { | "kp": { | "num": [ - | 0, + | 50, | "Percent$" | ], | "den": [ - | 1, + | 360, | "DegreesPerSecond$" | ] | }, @@ -136,7 +136,7 @@ object DefaultConfig { | "leftControlGains": { | "kp": { | "num": [ - | 10, + | 30, | "Percent$" | ], | "den": [ @@ -168,7 +168,7 @@ object DefaultConfig { | "rightControlGains": { | "kp": { | "num": [ - | 10, + | 30, | "Percent$" | ], | "den": [ @@ -207,32 +207,22 @@ object DefaultConfig { | ] | } | }, - | "agitator": { - | "ports": { - | "motor": 4 - | }, - | "properties": { - | "spinSpeed": [ - | -100, - | "Percent$" - | ] - | } - | }, + | "agitator": null, | "camSelect": { | "port": { | "leftCamPort": 5804, | "rightCamPort": 5805, - | "driveCamPort": 5803 + | "driveCamPort": 5811 | }, | "properties": { - | "coprocessorHostname": "10.8.46.19", - | "mjpegPath": "/stream.mjpg" + | "coprocessorHostname": "10.8.64.20", + | "mjpegPath": "/?action=stream" | } | }, | "climberPuller": { | "ports": { - | "motorChannelA": 15, - | "motorChannelB": 16 + | "motorChannelA": 14, + | "motorChannelB": 15 | }, | "props": { | "climbSpeed": [ @@ -241,178 +231,39 @@ object DefaultConfig { | ] | } | }, - | "collectorElevator": { - | "port": { - | "motor": 1 - | }, - | "properties": { - | "collectSpeed": [ - | 100, - | "Percent$" - | ] - | } - | }, - | "collectorExtender": { - | "port": { - | "pneumatic": 3 - | } - | }, - | "collectorRollers": { - | "ports": { - | "rollerChannel": 0 + | "collectorElevator": null, + | "collectorExtender": null, + | "collectorRollers": null, + | "gearRoller": { + | "ports":{ + | "motor": 16 | }, - | "properties": { - | "lowRollerSpeedOutput": [ - | 0, + | "props": { + | "defaultHoldingPower": [ + | 10, | "Percent$" | ], - | "highRollerSpeedOutput": [ + | "intakeGearPower": [ | 100, | "Percent$" - | ] - | } - | }, - | "gearGrabber": { - | "port": { - | "pneumatic": 1, - | "proximitySensor": 0 - | }, - | "props": { - | "detectingDistance": [ - | 2.2, - | "Volts$" - | ] - | } - | }, - | "gearTilter": { - | "port": { - | "pneumatic": 2 - | } - | }, - | "shooterFlywheel": { - | "ports": { - | "leftMotor": 2, - | "rightMotor": 3, - | "leftHall": 0, - | "rightHall": 1 - | }, - | "props": { - | "maxVelocityLeft": [ - | 6500, - | "RevolutionsPerMinute$" - | ], - | "maxVelocityRight": [ - | 6500, - | "RevolutionsPerMinute$" | ], - | "velocityGainsLeft": { - | "kp": { - | "num": [ - | 100, - | "Percent$" - | ], - | "den": [ - | 3000, - | "RevolutionsPerMinute$" - | ] - | }, - | "ki": { - | "num": [ - | 0, - | "Percent$" - | ], - | "den": [ - | 1000, - | "Each$" - | ] - | }, - | "kd": { - | "num": [ - | 0, - | "Percent$" - | ], - | "den": [ - | 1000, - | "RevolutionsPerMinute$ / s" - | ] - | } - | }, - | "velocityGainsRight": { - | "kp": { - | "num": [ - | 100, - | "Percent$" - | ], - | "den": [ - | 3000, - | "RevolutionsPerMinute$" - | ] - | }, - | "ki": { - | "num": [ - | 0, - | "Percent$" - | ], - | "den": [ - | 1000, - | "Each$" - | ] - | }, - | "kd": { - | "num": [ - | 0, - | "Percent$" - | ], - | "den": [ - | 1000, - | "RevolutionsPerMinute$ / s" - | ] - | } - | }, - | "lowShootSpeedLeft": [ - | 2000, - | "RevolutionsPerMinute$" - | ], - | "lowShootSpeedRight": [ - | 2000, - | "RevolutionsPerMinute$" - | ], - | "midShootSpeedLeft": [ - | 4300, - | "RevolutionsPerMinute$" - | ], - | "midShootSpeedRight": [ - | 4300, - | "RevolutionsPerMinute$" - | ], - | "fastShootSpeedLeft": [ - | 6000, - | "RevolutionsPerMinute$" - | ], - | "fastShootSpeedRight": [ - | 6000, - | "RevolutionsPerMinute$" - | ], - | "currentLimit": [ - | 40, + | "emitGearPower": [ + | -50, | "Percent$" | ], - | "speedTolerance": [ - | 75, - | "RevolutionsPerMinute$" + | "gearDetectionCurrent": [ + | 10, + | "Amperes$" | ] | } | }, - | "shooterShifter": { - | "ports": { + | "gearTilter": { + | "port": { | "pneumatic": 0 | } | }, - | "loadTray": { - | "port": { - | "pneumatic": 4 - | } - | } - |} - |""".stripMargin + | "shooterFlywheel": null, + | "shooterShifter": null, + | "loadTray": null + |}""".stripMargin } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala b/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala index 5ebfe70..050ae93 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala @@ -1,52 +1,63 @@ package com.lynbrookrobotics.seventeen -import java.io.File +import java.io.{File, PrintWriter} import com.google.common.reflect.ClassPath - import com.lynbrookrobotics.potassium.Signal -import com.lynbrookrobotics.potassium.config.TwoWayFile import com.lynbrookrobotics.potassium.config.SquantsPickling._ - import com.lynbrookrobotics.potassium.events.ImpulseEventSource import com.lynbrookrobotics.potassium.frc.WPIClock import com.lynbrookrobotics.potassium.streams.Stream - import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj.hal.HAL - import squants.time.Seconds - import upickle.default._ +import scala.io.Source + class LaunchRobot extends RobotBase { val targetFile = new File("/home/lvuser/robot-config.json") if (!targetFile.exists()) { targetFile.createNewFile() } - protected val configFile = new TwoWayFile(targetFile) - protected val parsedConfig = configFile.map { string => - val ret: RobotConfig = try { - read[RobotConfig](string) + def writeConfigFileValue(v: String): Unit = { + try { + val writer = new PrintWriter(targetFile) + writer.println(v) + writer.close() } catch { case e: Throwable => - println(s"Exception when reading config: $e") - read[RobotConfig](DefaultConfig.defaultConfig) + e.printStackTrace() } + } - ret - }( - (_, newValue) => write(newValue) - ) + private var latestConfigString: String = { + try { + Source.fromFile(targetFile).getLines().mkString("\n") + } catch { + case e: Throwable => + e.printStackTrace() + writeConfigFileValue(DefaultConfig.defaultConfig) + DefaultConfig.defaultConfig + } + } - private implicit val config = Signal { - parsedConfig.value + private var latestConfig: RobotConfig = { + try { + read[RobotConfig](latestConfigString) + } catch { + case e: Throwable => + e.printStackTrace() + writeConfigFileValue(DefaultConfig.defaultConfig) + latestConfigString = DefaultConfig.defaultConfig + read[RobotConfig](latestConfigString) + } } - implicit val clock = WPIClock + private implicit val config = Signal(latestConfig) - private implicit val hardware = RobotHardware(config.get) + implicit val clock = WPIClock private var coreRobot: CoreRobot = null @@ -62,18 +73,22 @@ class LaunchRobot extends RobotBase { } override def startCompetition(): Unit = { - WakeOnLan.awaken("B8:AE:ED:7E:78:E1") +// WakeOnLan.awaken("B8:AE:ED:7E:78:E1") + + implicit val hardware = RobotHardware(config.get) coreRobot = new CoreRobot( - Signal(configFile.value), + Signal(latestConfigString), newS => { - val oldS = configFile.value try { - configFile.value = newS + val readValue = read[RobotConfig](newS) + val writtenValue = write[RobotConfig](readValue) + writeConfigFileValue(writtenValue) + latestConfigString = writtenValue + latestConfig = readValue } catch { - case e: Throwable => - println(s"Unable to set new value for config, exception $e") - configFile.value = oldS + case e: Exception => + e.printStackTrace() } }, Stream.periodic(Seconds(0.01))(()) @@ -87,12 +102,14 @@ class LaunchRobot extends RobotBase { coreRobot.comms.foreach(_.connect()) - HAL.observeUserProgramStarting() - println("------------------------------------------\n" + "Finished preloading and establishing connections. " + "Wait 5 seconds to allow for sensor calibration\n") + Thread.sleep(5000) + + HAL.observeUserProgramStarting() + while (true) { ds.waitForData() eventPollingSource.fire() diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/RobotConfig.scala b/src/main/scala/com/lynbrookrobotics/seventeen/RobotConfig.scala index 60b12ed..7b63f7d 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/RobotConfig.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/RobotConfig.scala @@ -8,7 +8,7 @@ import com.lynbrookrobotics.seventeen.collector.extender.CollectorExtenderConfig import com.lynbrookrobotics.seventeen.collector.rollers.CollectorRollersConfig import com.lynbrookrobotics.seventeen.driver.DriverConfig import com.lynbrookrobotics.seventeen.drivetrain.DrivetrainConfig -import com.lynbrookrobotics.seventeen.gear.grabber.GearGrabberConfig +import com.lynbrookrobotics.seventeen.gear.roller.GearRollerConfig import com.lynbrookrobotics.seventeen.gear.tilter.GearTilterConfig import com.lynbrookrobotics.seventeen.loadtray.LoadTrayConfig import com.lynbrookrobotics.seventeen.shooter.flywheel.ShooterFlywheelConfig @@ -22,7 +22,7 @@ case class RobotConfig(driver: DriverConfig, collectorElevator: CollectorElevatorConfig, collectorExtender: CollectorExtenderConfig, collectorRollers: CollectorRollersConfig, - gearGrabber: GearGrabberConfig, + gearRoller: GearRollerConfig, gearTilter: GearTilterConfig, shooterFlywheel: ShooterFlywheelConfig, shooterShifter: ShooterShifterConfig, diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/RobotHardware.scala b/src/main/scala/com/lynbrookrobotics/seventeen/RobotHardware.scala index 93abb99..5be5975 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/RobotHardware.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/RobotHardware.scala @@ -9,7 +9,7 @@ import com.lynbrookrobotics.seventeen.collector.extender.CollectorExtenderHardwa import com.lynbrookrobotics.seventeen.collector.rollers.CollectorRollersHardware import com.lynbrookrobotics.seventeen.driver.DriverHardware import com.lynbrookrobotics.seventeen.drivetrain.DrivetrainHardware -import com.lynbrookrobotics.seventeen.gear.grabber.GearGrabberHardware +import com.lynbrookrobotics.seventeen.gear.roller.GearRollerHardware import com.lynbrookrobotics.seventeen.gear.tilter.GearTilterHardware import com.lynbrookrobotics.seventeen.loadtray.LoadTrayHardware import com.lynbrookrobotics.seventeen.shooter.flywheel.ShooterFlywheelHardware @@ -24,7 +24,7 @@ case class RobotHardware(driver: DriverHardware, collectorElevator: CollectorElevatorHardware, collectorExtender: CollectorExtenderHardware, collectorRollers: CollectorRollersHardware, - gearGrabber: GearGrabberHardware, + gearRoller: GearRollerHardware, gearTilter: GearTilterHardware, shooterFlywheel: ShooterFlywheelHardware, shooterShifter: ShooterShifterHardware, @@ -46,7 +46,7 @@ object RobotHardware { collectorElevator = if (collectorElevator != null) CollectorElevatorHardware(collectorElevator) else null, collectorExtender = if (collectorExtender != null) CollectorExtenderHardware(collectorExtender) else null, collectorRollers = if (collectorRollers != null) CollectorRollersHardware(collectorRollers) else null, - gearGrabber = if (gearGrabber != null) GearGrabberHardware(gearGrabber) else null, + gearRoller = if (gearRoller != null) GearRollerHardware(gearRoller) else null, gearTilter = if (gearTilter != null) GearTilterHardware(gearTilter) else null, shooterFlywheel = if (shooterFlywheel != null) ShooterFlywheelHardware(shooterFlywheel) else null, shooterShifter = if (shooterShifter != null) ShooterShifterHardware(shooterShifter) else null, diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/camselect/CamSelectHardware.scala b/src/main/scala/com/lynbrookrobotics/seventeen/camselect/CamSelectHardware.scala index 66f3eac..936be0a 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/camselect/CamSelectHardware.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/camselect/CamSelectHardware.scala @@ -20,7 +20,7 @@ object CamSelectHardware { camSelectHardware.mjpegServer.setSource(camSelectHardware.driverCam) - val cam = new HttpCamera("usbDriverCam", "http://10.8.46.2:1180/stream.mjpg") + val cam = new HttpCamera("usbDriverCam", "http://10.8.64.2:1180/stream.mjpg") CameraServer.getInstance.startAutomaticCapture(cam) camSelectHardware diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/driver/DriverHardware.scala b/src/main/scala/com/lynbrookrobotics/seventeen/driver/DriverHardware.scala index 47a88df..7701f51 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/driver/DriverHardware.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/driver/DriverHardware.scala @@ -26,7 +26,7 @@ object DriverHardware { new Joystick(config.operatorPort), new Joystick(config.driverWheelPort), new Joystick(config.launchpadPort), - DriverStation.getInstance() + Iterator.continually(DriverStation.getInstance()).find(_ != null).get ) } } \ No newline at end of file diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/package.scala b/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/package.scala index 8eaa3ea..4296dba 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/package.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/package.scala @@ -4,6 +4,7 @@ import com.lynbrookrobotics.potassium.commons.drivetrain._ import com.lynbrookrobotics.potassium.frc.Implicits._ import com.lynbrookrobotics.potassium.streams.Stream import com.lynbrookrobotics.potassium.{Component, Signal} +import edu.wpi.first.wpilibj.hal.PowerJNI import squants.Each import squants.electro.Volts import squants.time.Milliseconds @@ -54,10 +55,10 @@ package object drivetrain extends TwoSidedDrive(Milliseconds(5)) { self => override def defaultController: Stream[TwoSidedSignal] = self.defaultController - val normalDrivetrainVoltage = Volts(12) - override def applySignal(signal: TwoSidedSignal): Unit = { - val compFactor = normalDrivetrainVoltage / Volts(hardware.driverHardware.station.getBatteryVoltage) + val normalDrivetrainVoltage = Volts(12) + + val compFactor = normalDrivetrainVoltage / Volts(PowerJNI.getVinVoltage) output(hardware, TwoSidedSignal(signal.left * compFactor, signal.right * compFactor)) } } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/GearTasks.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/GearTasks.scala index edbe1f5..6a42cee 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/GearTasks.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/GearTasks.scala @@ -3,45 +3,20 @@ package com.lynbrookrobotics.seventeen.gear import com.lynbrookrobotics.potassium.Signal import com.lynbrookrobotics.potassium.clock.Clock import com.lynbrookrobotics.potassium.events.ImpulseEvent -import com.lynbrookrobotics.potassium.tasks.{FiniteTask, WaitTask} +import com.lynbrookrobotics.potassium.tasks.{ContinuousTask, FiniteTask, WaitTask} import com.lynbrookrobotics.seventeen.driver.DriverHardware -import com.lynbrookrobotics.seventeen.gear.grabber._ +import com.lynbrookrobotics.seventeen.gear.roller._ import com.lynbrookrobotics.seventeen.gear.tilter.{ExtendTilter, GearTilter, RetractTilter} import squants.time.Seconds object GearTasks { - def loadGearFromGroundAbortable(buttonTrigger: Int) - (tilter: GearTilter, - grabber: GearGrabber) - (implicit props: Signal[GearGrabberProperties], - hardware: GearGrabberHardware, - clock: Clock, - driverHardware: DriverHardware, polling: ImpulseEvent): FiniteTask = { - val pickUpGear = new OpenGrabberUntilGearAbortable(buttonTrigger)(grabber).andUntilDone( - new ExtendTilter(tilter) - ) - - val liftGear = new WaitTask(Seconds(0.3)).andUntilDone( - new CloseGrabber(grabber) and new ExtendTilter(tilter) - ).then(new WaitTask(Seconds(0.3)).andUntilDone(new RetractTilter(tilter))) - - pickUpGear.then(liftGear) + def collectGear(tilter: GearTilter, roller: GearRoller) + (implicit clock: Clock): FiniteTask = { + new CollectUntilGear(roller).andUntilDone(new ExtendTilter(tilter)) } - def slamDunk(tilter: GearTilter, - grabber: GearGrabber) - (implicit props: Signal[GearGrabberProperties], - hardware: GearGrabberHardware, - clock: Clock, - driverHardware: DriverHardware, polling: ImpulseEvent): FiniteTask = { - val pickUpGear = new WaitTask(Seconds(1)).andUntilDone( - new ExtendTilter(tilter) and new OpenGrabber(grabber) - ) - - val liftGear = new WaitTask(Seconds(0.3)).andUntilDone( - new CloseGrabber(grabber) and new ExtendTilter(tilter) - ).then(new WaitTask(Seconds(0.3)).andUntilDone(new RetractTilter(tilter))) - - pickUpGear.then(liftGear) + def scoreGear(tilter: GearTilter, roller: GearRoller) + (implicit clock: Clock): ContinuousTask = { + new RollOutwards(roller) and new ExtendTilter(tilter) } } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/CloseGrabber.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/CloseGrabber.scala deleted file mode 100644 index d7ff22a..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/CloseGrabber.scala +++ /dev/null @@ -1,13 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -import com.lynbrookrobotics.potassium.tasks.ContinuousTask - -class CloseGrabber(grabber: GearGrabber) extends ContinuousTask { - override protected def onStart(): Unit = { - grabber.setController(grabber.coreTicks.mapToConstant(GearGrabberClosed)) - } - - override protected def onEnd(): Unit = { - grabber.resetToDefault() - } -} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabber.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabber.scala deleted file mode 100644 index 17c5bfc..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabber.scala +++ /dev/null @@ -1,41 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -import com.lynbrookrobotics.potassium.streams.Stream -import com.lynbrookrobotics.potassium.{Component, Signal} -import com.lynbrookrobotics.seventeen.gear.tilter.GearTilter -import squants.time.Milliseconds - -sealed trait GearGrabberState - -case object GearGrabberOpen extends GearGrabberState - -case object GearGrabberClosed extends GearGrabberState - -class GearGrabber(val coreTicks: Stream[Unit])(implicit hardware: GearGrabberHardware, gearTilterF: () => Option[GearTilter]) - extends Component[GearGrabberState](Milliseconds(5)) { - - lazy val gearTilter = gearTilterF() - - override def defaultController: Stream[GearGrabberState] = coreTicks.mapToConstant(GearGrabberClosed) - - private var curLastOpenedTime: Long = 0 - val lastOpenTime = Signal(curLastOpenedTime) - - lazy val gearRetracted = gearTilter.map(_.lastRetractTime - .map(t => System.currentTimeMillis() - t <= 50/*1000*/)) - .getOrElse(Signal.constant(true)) - - override def applySignal(signal: GearGrabberState): Unit = { - val outWithSafety = if (gearRetracted.get) { - GearGrabberClosed - } else { - signal - } - - hardware.pneumatic.set(outWithSafety == GearGrabberOpen) - - if (outWithSafety == GearGrabberOpen) { - curLastOpenedTime = System.currentTimeMillis() - } - } -} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberConfig.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberConfig.scala deleted file mode 100644 index 9fd768d..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberConfig.scala +++ /dev/null @@ -1,3 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -case class GearGrabberConfig(port: GearGrabberPorts, props: GearGrabberProperties) diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberHardware.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberHardware.scala deleted file mode 100644 index 8516f94..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberHardware.scala +++ /dev/null @@ -1,16 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -import com.lynbrookrobotics.potassium.frc.ProximitySensor -import edu.wpi.first.wpilibj.Solenoid - -case class GearGrabberHardware(pneumatic: Solenoid, - proximitySensor: ProximitySensor) - -object GearGrabberHardware { - def apply(config: GearGrabberConfig): GearGrabberHardware = { - GearGrabberHardware( - pneumatic = new Solenoid(config.port.pneumatic), - proximitySensor = new ProximitySensor(config.port.proximitySensor) - ) - } -} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberPorts.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberPorts.scala deleted file mode 100644 index 0b44899..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberPorts.scala +++ /dev/null @@ -1,3 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -case class GearGrabberPorts(pneumatic: Int, proximitySensor: Int) diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberProperties.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberProperties.scala deleted file mode 100644 index cdc5851..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberProperties.scala +++ /dev/null @@ -1,5 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -import squants.electro.ElectricPotential - -case class GearGrabberProperties(detectingDistance: ElectricPotential) \ No newline at end of file diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabber.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabber.scala deleted file mode 100644 index 6e11a4b..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabber.scala +++ /dev/null @@ -1,13 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -import com.lynbrookrobotics.potassium.tasks.ContinuousTask - -class OpenGrabber(grabber: GearGrabber) extends ContinuousTask { - override protected def onStart(): Unit = { - grabber.setController(grabber.coreTicks.mapToConstant(GearGrabberOpen)) - } - - override protected def onEnd(): Unit = { - grabber.resetToDefault() - } -} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilGearAbortable.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilGearAbortable.scala deleted file mode 100644 index 6e6b2aa..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilGearAbortable.scala +++ /dev/null @@ -1,27 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -import com.lynbrookrobotics.potassium.Signal -import com.lynbrookrobotics.potassium.events.ImpulseEvent -import com.lynbrookrobotics.potassium.tasks.FiniteTask -import com.lynbrookrobotics.seventeen.driver.DriverHardware - -class OpenGrabberUntilGearAbortable(button: Int) - (grabber: GearGrabber) - (implicit hardware: GearGrabberHardware, - props: Signal[GearGrabberProperties], driverHardware: DriverHardware, - polling: ImpulseEvent) extends FiniteTask { - val proximitySensor = hardware.proximitySensor - - override protected def onStart(): Unit = { - grabber.setController(grabber.coreTicks.mapToConstant[GearGrabberState](GearGrabberOpen).withCheck { _ => - if (proximitySensor.getVoltage > props.get.detectingDistance.toVolts || - !driverHardware.operatorJoystick.getRawButton(button)) { - finished() - } - }) - } - - override protected def onEnd(): Unit = { - grabber.resetToDefault() - } -} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilHasGear.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilHasGear.scala deleted file mode 100644 index 512a2f6..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilHasGear.scala +++ /dev/null @@ -1,21 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -import com.lynbrookrobotics.potassium.Signal -import com.lynbrookrobotics.potassium.tasks.FiniteTask - -class OpenGrabberUntilHasGear(implicit hardware: GearGrabberHardware, grabber: GearGrabber, - props: Signal[GearGrabberProperties]) extends FiniteTask { - val proximitySensor = hardware.proximitySensor - - override protected def onStart(): Unit = { - grabber.setController(grabber.coreTicks.mapToConstant[GearGrabberState](GearGrabberOpen).withCheck { _ => - if (proximitySensor.getVoltage > props.get.detectingDistance.toVolts) { - finished() - } - }) - } - - override protected def onEnd(): Unit = { - grabber.resetToDefault() - } -} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilReleased.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilReleased.scala deleted file mode 100644 index 25d6b73..0000000 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilReleased.scala +++ /dev/null @@ -1,24 +0,0 @@ -package com.lynbrookrobotics.seventeen.gear.grabber - -import com.lynbrookrobotics.potassium.Signal -import com.lynbrookrobotics.potassium.events.ImpulseEvent -import com.lynbrookrobotics.potassium.tasks.FiniteTask -import com.lynbrookrobotics.seventeen.driver.DriverHardware - -class OpenGrabberUntilReleased(implicit hardware: GearGrabberHardware, grabber: GearGrabber, - props: Signal[GearGrabberProperties], driverHardware: DriverHardware, - polling: ImpulseEvent) extends FiniteTask { - val proximitySensor = hardware.proximitySensor - - override protected def onStart(): Unit = { - grabber.setController(grabber.coreTicks.mapToConstant[GearGrabberState](GearGrabberOpen).withCheck { _ => - if (proximitySensor.getVoltage < props.get.detectingDistance.toVolts) { - finished() - } - }) - } - - override protected def onEnd(): Unit = { - grabber.resetToDefault() - } -} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/CollectUntilGear.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/CollectUntilGear.scala new file mode 100644 index 0000000..d3e80bd --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/CollectUntilGear.scala @@ -0,0 +1,40 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.lynbrookrobotics.potassium.commons.electronics.CurrentLimiting +import com.lynbrookrobotics.potassium.tasks.{ContinuousTask, FiniteTask} +import squants.Percent +import squants.electro.Amperes +import squants.time.Seconds + +class CollectUntilGear(roller: GearRoller) extends FiniteTask { + var cancelHandle: () => Unit = null + var ticksAboveLimit = 0 + + override protected def onStart(): Unit = { + roller.disabledAutoRun = false + val currentStream = roller.coreTicks.map(_ => Amperes(roller.hardware.motor.getOutputCurrent)) + + cancelHandle = currentStream.foreach { current => + if (current >= roller.properties.get.gearDetectionCurrent) { + ticksAboveLimit += 1 + if (ticksAboveLimit >= 15) { + finished() + } + } else { + ticksAboveLimit = 0 + } + } + + roller.setController(CurrentLimiting.slewRate( + roller.coreTicks.map(_ => roller.properties.get.intakeGearPower), + Percent(100) / Seconds(1) + )) + } + + override protected def onEnd(): Unit = { + cancelHandle.apply() + cancelHandle = null + ticksAboveLimit = 0 + roller.resetToDefault() + } +} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRoller.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRoller.scala new file mode 100644 index 0000000..f02d828 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRoller.scala @@ -0,0 +1,25 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.lynbrookrobotics.potassium.commons.electronics.CurrentLimiting +import com.lynbrookrobotics.potassium.streams.Stream +import com.lynbrookrobotics.potassium.{Component, Signal} +import squants.{Dimensionless, Percent} +import squants.time.{Milliseconds, Seconds} + +class GearRoller(val coreTicks: Stream[Unit])(implicit val properties: Signal[GearRollerProperties], + val hardware: GearRollerHardware) + extends Component[Dimensionless](Milliseconds(5)) { + var disabledAutoRun = false + + override def setController(controller: Stream[Dimensionless]): Unit = { + super.setController(CurrentLimiting.slewRate(controller, Percent(100) / Seconds(0.3))) + } + + override def defaultController: Stream[Dimensionless] = coreTicks.map(_ => { + if (disabledAutoRun) Percent(0) else properties.get.defaultHoldingPower + }) + + override def applySignal(signal: Dimensionless): Unit = { + hardware.motor.set(signal.toEach) + } +} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerConfig.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerConfig.scala new file mode 100644 index 0000000..2240b15 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerConfig.scala @@ -0,0 +1,3 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +case class GearRollerConfig(ports: GearRollerPorts, props: GearRollerProperties) diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerHardware.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerHardware.scala new file mode 100644 index 0000000..3b81db3 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerHardware.scala @@ -0,0 +1,17 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.ctre.CANTalon + +case class GearRollerHardware(motor: CANTalon) + +object GearRollerHardware { + def apply(config: GearRollerConfig): GearRollerHardware = { + GearRollerHardware( + motor = { + val talon = new CANTalon(config.ports.motor) + talon.setInverted(true) + talon + } + ) + } +} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerPorts.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerPorts.scala new file mode 100644 index 0000000..21714c4 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerPorts.scala @@ -0,0 +1,3 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +case class GearRollerPorts(motor: Int) diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerProperties.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerProperties.scala new file mode 100644 index 0000000..6d7e2e2 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerProperties.scala @@ -0,0 +1,8 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import squants.Dimensionless +import squants.electro.{ElectricCurrent, ElectricPotential} + +case class GearRollerProperties(defaultHoldingPower: Dimensionless, + intakeGearPower: Dimensionless, emitGearPower: Dimensionless, + gearDetectionCurrent: ElectricCurrent) \ No newline at end of file diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollInwards.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollInwards.scala new file mode 100644 index 0000000..0c6a7b8 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollInwards.scala @@ -0,0 +1,14 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.lynbrookrobotics.potassium.tasks.ContinuousTask + +class RollInwards(roller: GearRoller) extends ContinuousTask { + override protected def onStart(): Unit = { + roller.disabledAutoRun = false + roller.setController(roller.coreTicks.map(_ => roller.properties.get.intakeGearPower)) + } + + override protected def onEnd(): Unit = { + roller.resetToDefault() + } +} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollOutwards.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollOutwards.scala new file mode 100644 index 0000000..1ab11f4 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollOutwards.scala @@ -0,0 +1,14 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.lynbrookrobotics.potassium.tasks.ContinuousTask + +class RollOutwards(roller: GearRoller) extends ContinuousTask { + override protected def onStart(): Unit = { + roller.disabledAutoRun = false + roller.setController(roller.coreTicks.map(_ => roller.properties.get.emitGearPower)) + } + + override protected def onEnd(): Unit = { + roller.resetToDefault() + } +} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/tilter/GearTilter.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/tilter/GearTilter.scala index 9a4da38..9693761 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/tilter/GearTilter.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/tilter/GearTilter.scala @@ -3,7 +3,6 @@ package com.lynbrookrobotics.seventeen.gear.tilter import com.lynbrookrobotics.potassium.streams.Stream import com.lynbrookrobotics.potassium.{Component, Signal} import com.lynbrookrobotics.seventeen.collector.extender.CollectorExtender -import com.lynbrookrobotics.seventeen.gear.grabber.GearGrabber import squants.time.Milliseconds sealed trait GearTilterState @@ -13,7 +12,7 @@ case object GearTilterExtended extends GearTilterState case object GearTilterRetracted extends GearTilterState class GearTilter(val coreTicks: Stream[Unit], - gearGrabber: => Option[GearGrabber], collectorExtender: => Option[CollectorExtender]) + collectorExtender: => Option[CollectorExtender]) (implicit hardware: GearTilterHardware) extends Component[GearTilterState](Milliseconds(5)) { override def defaultController: Stream[GearTilterState] = coreTicks.mapToConstant(GearTilterRetracted) @@ -29,14 +28,8 @@ class GearTilter(val coreTicks: Stream[Unit], private var curLastOpenTime: Long = 0 val lastOpenTime = Signal(curLastOpenTime) - lazy val gearOpened = gearGrabber.map(_.lastOpenTime. - map(t => System.currentTimeMillis() - t <= 50)). - getOrElse(Signal.constant(false)) - override def applySignal(signal: GearTilterState): Unit = { - val outWithSafety = if (gearOpened.get) { - GearTilterExtended - } else if (collectorExtended.get) { + val outWithSafety = if (collectorExtended.get) { GearTilterRetracted } else { signal diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/shooter/flywheel/ShooterFlywheel.scala b/src/main/scala/com/lynbrookrobotics/seventeen/shooter/flywheel/ShooterFlywheel.scala index 13f671e..77f3860 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/shooter/flywheel/ShooterFlywheel.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/shooter/flywheel/ShooterFlywheel.scala @@ -13,6 +13,9 @@ class ShooterFlywheel(val coreTicks: Stream[Unit]) extends Component[DoubleFlywheelSignal](Milliseconds(5)) { val NominalVoltage = 11.9 + val midShootSpeedLeft = coreTicks.map(_ => properties.get.midShootSpeedLeft) + val midShootSpeedRight = coreTicks.map(_ => properties.get.midShootSpeedRight) + override def defaultController: Stream[DoubleFlywheelSignal] = coreTicks.mapToConstant { DoubleFlywheelSignal(Each(0), Each(0)) } diff --git a/src/test/scala/com/lynbrookrobotics/seventeen/ConfigGenerator.scala b/src/test/scala/com/lynbrookrobotics/seventeen/ConfigGenerator.scala index 3adac5a..b249662 100644 --- a/src/test/scala/com/lynbrookrobotics/seventeen/ConfigGenerator.scala +++ b/src/test/scala/com/lynbrookrobotics/seventeen/ConfigGenerator.scala @@ -11,18 +11,17 @@ import com.lynbrookrobotics.seventeen.collector.extender.{CollectorExtenderConfi import com.lynbrookrobotics.seventeen.collector.rollers.{CollectorRollersConfig, CollectorRollersPorts, CollectorRollersProperties} import com.lynbrookrobotics.seventeen.driver.DriverConfig import com.lynbrookrobotics.seventeen.drivetrain.{DrivetrainConfig, DrivetrainPorts, DrivetrainProperties} -import com.lynbrookrobotics.seventeen.gear.grabber.{GearGrabberConfig, GearGrabberPorts, GearGrabberProperties} +import com.lynbrookrobotics.seventeen.gear.roller.{GearRollerConfig, GearRollerPorts, GearRollerProperties} import com.lynbrookrobotics.seventeen.gear.tilter.{GearTilterConfig, GearTilterPorts} import com.lynbrookrobotics.seventeen.loadtray.{LoadTrayConfig, LoadTrayPorts} import com.lynbrookrobotics.seventeen.shooter.flywheel.{ShooterFlywheelConfig, ShooterFlywheelPorts, ShooterFlywheelProperties} import com.lynbrookrobotics.seventeen.shooter.shifter.{ShooterShifterConfig, ShooterShifterPorts} -import squants.electro.Volts +import squants.electro.{Amperes, Volts} import squants.motion.{DegreesPerSecond, FeetPerSecond, MetersPerSecondSquared} import squants.space._ import squants.time.{RevolutionsPerMinute, Seconds} import squants.{Each, Percent} import upickle.default._ - import com.lynbrookrobotics.potassium.config.SquantsPickling._ object ConfigGenerator extends App { @@ -126,13 +125,15 @@ object ConfigGenerator extends App { highRollerSpeedOutput = Percent(0) ) ), - gearGrabber = GearGrabberConfig( - port = GearGrabberPorts( - pneumatic = 0, - proximitySensor = 0 + gearRoller = GearRollerConfig( + ports = GearRollerPorts( + motor = 0 ), - props = GearGrabberProperties( - detectingDistance = Volts(0) + props = GearRollerProperties( + defaultHoldingPower = Percent(10), + intakeGearPower = Percent(100), + emitGearPower = Percent(50), + gearDetectionCurrent = Amperes(1000) ) ), gearTilter = GearTilterConfig(