From 6704a33337a2526f994d42e55dc35973c1a03bf2 Mon Sep 17 00:00:00 2001 From: Shadaj Laddad Date: Wed, 11 Oct 2017 17:49:43 -0700 Subject: [PATCH 01/11] Add code for controlling gear rollers --- .../seventeen/AutoGenerator.scala | 27 ++++++------ .../seventeen/ButtonMappings.scala | 30 ++++---------- .../seventeen/CoreRobot.scala | 35 ++++++++-------- .../seventeen/RobotConfig.scala | 4 +- .../seventeen/RobotHardware.scala | 6 +-- .../seventeen/gear/GearTasks.scala | 41 ++++--------------- .../seventeen/gear/grabber/CloseGrabber.scala | 13 ------ .../seventeen/gear/grabber/GearGrabber.scala | 41 ------------------- .../gear/grabber/GearGrabberConfig.scala | 3 -- .../gear/grabber/GearGrabberHardware.scala | 16 -------- .../gear/grabber/GearGrabberPorts.scala | 3 -- .../gear/grabber/GearGrabberProperties.scala | 5 --- .../seventeen/gear/grabber/OpenGrabber.scala | 13 ------ .../OpenGrabberUntilGearAbortable.scala | 27 ------------ .../grabber/OpenGrabberUntilHasGear.scala | 21 ---------- .../grabber/OpenGrabberUntilReleased.scala | 24 ----------- .../gear/roller/CollectUntilGear.scala | 18 ++++++++ .../seventeen/gear/roller/GearRoller.scala | 17 ++++++++ .../gear/roller/GearRollerConfig.scala | 3 ++ .../gear/roller/GearRollerHardware.scala | 13 ++++++ .../gear/roller/GearRollerPorts.scala | 3 ++ .../gear/roller/GearRollerProperties.scala | 8 ++++ .../seventeen/gear/roller/RollInwards.scala | 13 ++++++ .../seventeen/gear/roller/RollOutwards.scala | 13 ++++++ .../seventeen/gear/tilter/GearTilter.scala | 11 +---- .../seventeen/ConfigGenerator.scala | 19 +++++---- 26 files changed, 151 insertions(+), 276 deletions(-) delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/CloseGrabber.scala delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabber.scala delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberConfig.scala delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberHardware.scala delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberPorts.scala delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/GearGrabberProperties.scala delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabber.scala delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilGearAbortable.scala delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilHasGear.scala delete mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/grabber/OpenGrabberUntilReleased.scala create mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/CollectUntilGear.scala create mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRoller.scala create mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerConfig.scala create mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerHardware.scala create mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerPorts.scala create mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerProperties.scala create mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollInwards.scala create mode 100644 src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollOutwards.scala diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala b/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala index e55ba7a..a5bd3ab 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 @@ -38,31 +39,31 @@ 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) + 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, Inches(3), Degrees(10), Percent(30) )(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), @@ -78,11 +79,11 @@ class AutoGenerator(r: CoreRobot) { Degrees(10), Percent(30) )(drivetrain).withTimeout(Seconds(5))).then( - toGearAndDrop(drivetrain, gearGrabber, gearTilter) + toGearAndDrop(drivetrain, gearRoller, gearTilter) ) } - 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), @@ -98,7 +99,7 @@ class AutoGenerator(r: CoreRobot) { Degrees(10), Percent(30) )(drivetrain).withTimeout(Seconds(5))).then( - toGearAndDrop(drivetrain, gearGrabber, gearTilter) + toGearAndDrop(drivetrain, gearRoller, gearTilter) ) } @@ -284,7 +285,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 +302,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 +334,7 @@ class AutoGenerator(r: CoreRobot) { } def shootCenterGear(drivetrain: Drivetrain, - gearGrabber: GearGrabber, + gearRoller: GearRoller, gearTilter: GearTilter, collectorElevator: CollectorElevator, collectorRollers: CollectorRollers, @@ -346,6 +347,6 @@ class AutoGenerator(r: CoreRobot) { midShootSpeedLeft, 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..75ee503 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala @@ -8,8 +8,7 @@ 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.tilter.ExtendTilter +import com.lynbrookrobotics.seventeen.gear.GearTasks import com.lynbrookrobotics.seventeen.loadtray.ExtendTray import com.lynbrookrobotics.seventeen.shooter.ShooterTasks import com.lynbrookrobotics.seventeen.shooter.flywheel.velocityTasks.{WhileAtDoubleVelocity, WhileAtVelocity} @@ -18,7 +17,6 @@ import squants.Percent import squants.time._ class ButtonMappings(r: CoreRobot) { - import r._ var curFlywheelTargetLeft: Frequency = if (config.get.shooterFlywheel != null) { @@ -138,37 +136,23 @@ 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) /** - * Releases gear + * Collects gear * only RightFour pressed */ - onlyRightFourPressed.foreach(new OpenGrabber(gearGrabber)) + rightFourPressed.foreach(GearTasks.collectGear(gearTilter, gearRoller).toContinuous) /** * Extends tilter * only RightFive pressed */ - onlyRightFivePressed.foreach(new ExtendTilter(gearTilter)) - - /** - * Extends tilter and opens grabber - * both RightFour and RightFive pressed - */ - bothPressed.foreach( - new OpenGrabber(gearGrabber).and(new ExtendTilter(gearTilter)) - ) + rightFivePressed.foreach(GearTasks.scoreGear(gearTilter, gearRoller)) } for { diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala b/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala index 2d7ceb9..66bfac3 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,18 +77,17 @@ 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 } // 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 // Shooter Flywheel @@ -114,7 +113,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 +147,7 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni collectorElevator, collectorExtender, collectorRollers, - gearGrabber, + gearRoller, gearTilter, shooterFlywheel, shooterShifter, @@ -181,23 +180,23 @@ 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) { @@ -211,7 +210,7 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni for { drivetrain <- drivetrain - gearGrabber <- gearGrabber + gearGrabber <- gearRoller gearTilter <- gearTilter collectorElevator <- collectorElevator collectorRollers <- collectorRollers @@ -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/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/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..bc02594 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/CollectUntilGear.scala @@ -0,0 +1,18 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.lynbrookrobotics.potassium.tasks.{ContinuousTask, FiniteTask} +import squants.electro.Amperes + +class CollectUntilGear(roller: GearRoller) extends FiniteTask { + override protected def onStart(): Unit = { + roller.setController(roller.coreTicks.map(_ => roller.properties.get.intakeGearPower).withCheck { _ => + if (Amperes(roller.hardware.motor.getOutputCurrent) >= roller.properties.get.gearDetectionCurrent) { + finished() + } + }) + } + + override protected def onEnd(): Unit = { + 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..7d0f11b --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRoller.scala @@ -0,0 +1,17 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.lynbrookrobotics.potassium.streams.Stream +import com.lynbrookrobotics.potassium.{Component, Signal} +import squants.Dimensionless +import squants.time.Milliseconds + +class GearRoller(val coreTicks: Stream[Unit])(implicit val properties: Signal[GearRollerProperties], + val hardware: GearRollerHardware) + extends Component[Dimensionless](Milliseconds(5)) { + + override def defaultController: Stream[Dimensionless] = coreTicks.map(_ => 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..8139cb8 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerHardware.scala @@ -0,0 +1,13 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.ctre.CANTalon + +case class GearRollerHardware(motor: CANTalon) + +object GearRollerHardware { + def apply(config: GearRollerConfig): GearRollerHardware = { + GearRollerHardware( + motor = new CANTalon(config.ports.motor) + ) + } +} 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..62fe2f1 --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollInwards.scala @@ -0,0 +1,13 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.lynbrookrobotics.potassium.tasks.ContinuousTask + +class RollInwards(roller: GearRoller) extends ContinuousTask { + override protected def onStart(): Unit = { + 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..047e98c --- /dev/null +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollOutwards.scala @@ -0,0 +1,13 @@ +package com.lynbrookrobotics.seventeen.gear.roller + +import com.lynbrookrobotics.potassium.tasks.ContinuousTask + +class RollOutwards(roller: GearRoller) extends ContinuousTask { + override protected def onStart(): Unit = { + 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/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( From 1f17895722f9f00496fb3cfe7f45597f2b32b78b Mon Sep 17 00:00:00 2001 From: Shadaj Laddad Date: Wed, 11 Oct 2017 18:56:27 -0700 Subject: [PATCH 02/11] Update config and eliminate two-way file handling --- 864-robot-config.json | 27 +++++-- .../seventeen/DefaultConfig.scala | 77 +++++++++++-------- .../seventeen/LaunchRobot.scala | 65 ++++++++++------ 3 files changed, 103 insertions(+), 66 deletions(-) diff --git a/864-robot-config.json b/864-robot-config.json index a2822ae..4f81f91 100644 --- a/864-robot-config.json +++ b/864-robot-config.json @@ -269,15 +269,26 @@ ] } }, - "gearGrabber": { - "port": { - "pneumatic": 1, - "proximitySensor": 0 + "gearRoller":{ + "ports":{ + "motor":0 }, - "props": { - "detectingDistance": [ - 2.2, - "Volts$" + "props":{ + "defaultHoldingPower":[ + 10, + "Percent$" + ], + "intakeGearPower":[ + 100, + "Percent$" + ], + "emitGearPower":[ + -50, + "Percent$" + ], + "gearDetectionCurrent":[ + 1000, + "Amperes$" ] } }, diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala b/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala index c5bd325..3e2929e 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": [ @@ -209,11 +209,11 @@ object DefaultConfig { | }, | "agitator": { | "ports": { - | "motor": 4 + | "motor": 2 | }, | "properties": { | "spinSpeed": [ - | -100, + | -50, | "Percent$" | ] | } @@ -231,8 +231,8 @@ object DefaultConfig { | }, | "climberPuller": { | "ports": { - | "motorChannelA": 15, - | "motorChannelB": 16 + | "motorChannelA": 14, + | "motorChannelB": 15 | }, | "props": { | "climbSpeed": [ @@ -272,27 +272,38 @@ object DefaultConfig { | ] | } | }, - | "gearGrabber": { - | "port": { - | "pneumatic": 1, - | "proximitySensor": 0 + | "gearRoller":{ + | "ports":{ + | "motor":0 | }, - | "props": { - | "detectingDistance": [ - | 2.2, - | "Volts$" + | "props":{ + | "defaultHoldingPower":[ + | 10, + | "Percent$" + | ], + | "intakeGearPower":[ + | 100, + | "Percent$" + | ], + | "emitGearPower":[ + | -50, + | "Percent$" + | ], + | "gearDetectionCurrent":[ + | 1000, + | "Amperes$" | ] | } | }, | "gearTilter": { | "port": { - | "pneumatic": 2 + | "pneumatic": 0 | } | }, | "shooterFlywheel": { | "ports": { - | "leftMotor": 2, - | "rightMotor": 3, + | "leftMotor": 3, + | "rightMotor": 4, | "leftHall": 0, | "rightHall": 1 | }, @@ -370,31 +381,31 @@ object DefaultConfig { | } | }, | "lowShootSpeedLeft": [ - | 2000, + | 3650, | "RevolutionsPerMinute$" | ], | "lowShootSpeedRight": [ - | 2000, + | 3650, | "RevolutionsPerMinute$" | ], | "midShootSpeedLeft": [ - | 4300, + | 3700, | "RevolutionsPerMinute$" | ], | "midShootSpeedRight": [ - | 4300, + | 3700, | "RevolutionsPerMinute$" | ], | "fastShootSpeedLeft": [ - | 6000, + | 3750, | "RevolutionsPerMinute$" | ], | "fastShootSpeedRight": [ - | 6000, + | 3750, | "RevolutionsPerMinute$" | ], | "currentLimit": [ - | 40, + | 50, | "Percent$" | ], | "speedTolerance": [ @@ -405,7 +416,7 @@ object DefaultConfig { | }, | "shooterShifter": { | "ports": { - | "pneumatic": 0 + | "pneumatic": 2 | } | }, | "loadTray": { diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala b/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala index 5ebfe70..599d9ff 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala @@ -1,49 +1,62 @@ 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) + } } + private implicit val config = Signal(latestConfig) + implicit val clock = WPIClock private implicit val hardware = RobotHardware(config.get) @@ -65,15 +78,17 @@ class LaunchRobot extends RobotBase { WakeOnLan.awaken("B8:AE:ED:7E:78:E1") 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))(()) From 7bc3d56e8907ad9f90999c10357912e482fa0ed4 Mon Sep 17 00:00:00 2001 From: Shadaj Laddad Date: Wed, 11 Oct 2017 18:58:16 -0700 Subject: [PATCH 03/11] Actually wait for five seconds for calibration --- .../scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala b/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala index 599d9ff..5bcf6e1 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala @@ -102,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() From 7c18ce8ae86978d98d2af13ca210ca822480a535 Mon Sep 17 00:00:00 2001 From: Shadaj Laddad Date: Wed, 11 Oct 2017 19:43:08 -0700 Subject: [PATCH 04/11] Make things compile --- build.sbt | 2 +- .../com/lynbrookrobotics/seventeen/AutoGenerator.scala | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) 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/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala b/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala index a5bd3ab..35823b5 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala @@ -104,7 +104,7 @@ class AutoGenerator(r: CoreRobot) { } def leftGearPurePursuit(drivetrain: Drivetrain, - gearGrabber: GearGrabber, + gearRoller: GearRoller, gearTilter: GearTilter): FiniteTask = { val relativeTurn = drivetrainHardware.turnPosition.relativize((init, curr) => { curr - init @@ -131,12 +131,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 }) @@ -162,7 +162,7 @@ class AutoGenerator(r: CoreRobot) { turnPosition = relativeTurn, steadyOutput = Percent(50) )(drivetrain).withTimeout(Seconds(5)).then( - toGearAndDrop(drivetrain, gearGrabber, gearTilter) + toGearAndDrop(drivetrain, gearRoller, gearTilter) ) } From 1779cfc7a0387bc7a025448cbcf96ef1a0b1ae03 Mon Sep 17 00:00:00 2001 From: Shadaj Laddad Date: Wed, 11 Oct 2017 21:17:20 -0700 Subject: [PATCH 05/11] Complete roller gear tested on the robot --- 864-robot-config.json | 189 ++---------------- .../seventeen/AutoGenerator.scala | 26 ++- .../seventeen/ButtonMappings.scala | 6 + .../seventeen/CoreRobot.scala | 10 +- .../gear/roller/CollectUntilGear.scala | 30 ++- .../seventeen/gear/roller/GearRoller.scala | 14 +- .../gear/roller/GearRollerHardware.scala | 6 +- .../seventeen/gear/roller/RollInwards.scala | 1 + .../seventeen/gear/roller/RollOutwards.scala | 1 + .../shooter/flywheel/ShooterFlywheel.scala | 3 + 10 files changed, 87 insertions(+), 199 deletions(-) diff --git a/864-robot-config.json b/864-robot-config.json index 4f81f91..d37a88c 100644 --- a/864-robot-config.json +++ b/864-robot-config.json @@ -204,17 +204,7 @@ ] } }, - "agitator": { - "ports": { - "motor": 2 - }, - "properties": { - "spinSpeed": [ - -50, - "Percent$" - ] - } - }, + "agitator": null, "camSelect": { "port": { "leftCamPort": 5804, @@ -238,56 +228,28 @@ ] } }, - "collectorElevator": { - "port": { - "motor": 1 - }, - "properties": { - "collectSpeed": [ - 100, - "Percent$" - ] - } - }, - "collectorExtender": { - "port": { - "pneumatic": 3 - } - }, - "collectorRollers": { - "ports": { - "rollerChannel": 0 - }, - "properties": { - "lowRollerSpeedOutput": [ - 0, - "Percent$" - ], - "highRollerSpeedOutput": [ - 100, - "Percent$" - ] - } - }, - "gearRoller":{ + "collectorElevator": null, + "collectorExtender": null, + "collectorRollers": null, + "gearRoller": { "ports":{ - "motor":0 + "motor": 16 }, - "props":{ - "defaultHoldingPower":[ + "props": { + "defaultHoldingPower": [ 10, "Percent$" ], - "intakeGearPower":[ + "intakeGearPower": [ 100, "Percent$" ], - "emitGearPower":[ + "emitGearPower": [ -50, "Percent$" ], - "gearDetectionCurrent":[ - 1000, + "gearDetectionCurrent": [ + 10, "Amperes$" ] } @@ -297,128 +259,7 @@ "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, - "Percent$" - ], - "speedTolerance": [ - 75, - "RevolutionsPerMinute$" - ] - } - }, - "shooterShifter": { - "ports": { - "pneumatic": 2 - } - }, - "loadTray": { - "port": { - "pneumatic": 4 - } - } + "shooterFlywheel": null, + "shooterShifter": null, + "loadTray": null } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala b/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala index 35823b5..6daa6f3 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala @@ -27,9 +27,6 @@ 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 = { new DriveDistanceStraight( Inches(107) - robotLength, @@ -203,10 +200,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( @@ -216,6 +213,7 @@ class AutoGenerator(r: CoreRobot) { Percent(30) )(drivetrain).withTimeout(Seconds(3))) } + def leftHopperAndShoot(drivetrain: Drivetrain, collectorElevator: CollectorElevator, collectorRollers: CollectorRollers, @@ -226,10 +224,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( @@ -250,10 +248,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( @@ -344,8 +342,8 @@ 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, gearRoller, gearTilter)) } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala b/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala index 75ee503..14ce677 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala @@ -141,6 +141,7 @@ class ButtonMappings(r: CoreRobot) { } { val rightFourPressed = driverHardware.operatorJoystick.buttonPressed(JoystickButtons.RightFour) val rightFivePressed = driverHardware.operatorJoystick.buttonPressed(JoystickButtons.RightFive) + val rightSixPressed = driverHardware.operatorJoystick.buttonPressed(JoystickButtons.RightSix) /** * Collects gear @@ -153,6 +154,11 @@ class ButtonMappings(r: CoreRobot) { * only RightFive pressed */ rightFivePressed.foreach(GearTasks.scoreGear(gearTilter, gearRoller)) + + rightSixPressed.onStart.foreach(() => { + println("DISABLING AUTO RUN") + gearRoller.disabledAutoRun = true + }) } for { diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala b/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala index 66bfac3..0303ce6 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala @@ -83,6 +83,8 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni if (config.get.gearRoller != null) Some(new GearRoller(coreTicks)) else None } + println(gearRoller) + // Gear Tilter implicit val gearTilterHardware = hardware.gearTilter val gearTilter: Option[GearTilter] = @@ -90,6 +92,8 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni Some(new GearTilter(coreTicks, collectorExtender)) } else None + println(gearTilter) + // Shooter Flywheel implicit val shooterFlywheelHardware = hardware.shooterFlywheel implicit val shooterFlywheelProps = config.map(_.shooterFlywheel.props) @@ -200,11 +204,11 @@ class CoreRobot(configFileValue: Signal[String], updateConfigFile: String => Uni ) 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 } } @@ -300,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 } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/CollectUntilGear.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/CollectUntilGear.scala index bc02594..d3e80bd 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/CollectUntilGear.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/CollectUntilGear.scala @@ -1,18 +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.setController(roller.coreTicks.map(_ => roller.properties.get.intakeGearPower).withCheck { _ => - if (Amperes(roller.hardware.motor.getOutputCurrent) >= roller.properties.get.gearDetectionCurrent) { - finished() + 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 index 7d0f11b..f02d828 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRoller.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRoller.scala @@ -1,15 +1,23 @@ 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 -import squants.time.Milliseconds +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 defaultController: Stream[Dimensionless] = coreTicks.map(_ => properties.get.defaultHoldingPower) + 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/GearRollerHardware.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerHardware.scala index 8139cb8..3b81db3 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerHardware.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/GearRollerHardware.scala @@ -7,7 +7,11 @@ case class GearRollerHardware(motor: CANTalon) object GearRollerHardware { def apply(config: GearRollerConfig): GearRollerHardware = { GearRollerHardware( - motor = new CANTalon(config.ports.motor) + motor = { + val talon = new CANTalon(config.ports.motor) + talon.setInverted(true) + talon + } ) } } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollInwards.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollInwards.scala index 62fe2f1..0c6a7b8 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollInwards.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollInwards.scala @@ -4,6 +4,7 @@ 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)) } diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollOutwards.scala b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollOutwards.scala index 047e98c..1ab11f4 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollOutwards.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/gear/roller/RollOutwards.scala @@ -4,6 +4,7 @@ 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)) } 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)) } From dd6b7cd271fb8e1cc0503b53ab23618844336d74 Mon Sep 17 00:00:00 2001 From: Shadaj Laddad Date: Wed, 11 Oct 2017 21:25:17 -0700 Subject: [PATCH 06/11] Update auto/manual collection button mappings --- .../com/lynbrookrobotics/seventeen/ButtonMappings.scala | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala b/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala index 14ce677..56a40e2 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/ButtonMappings.scala @@ -9,6 +9,8 @@ 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.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 import com.lynbrookrobotics.seventeen.shooter.flywheel.velocityTasks.{WhileAtDoubleVelocity, WhileAtVelocity} @@ -147,7 +149,7 @@ class ButtonMappings(r: CoreRobot) { * Collects gear * only RightFour pressed */ - rightFourPressed.foreach(GearTasks.collectGear(gearTilter, gearRoller).toContinuous) + rightFourPressed.foreach(new RollInwards(gearRoller) and new ExtendTilter(gearTilter)) /** * Extends tilter @@ -159,6 +161,11 @@ class ButtonMappings(r: CoreRobot) { println("DISABLING AUTO RUN") gearRoller.disabledAutoRun = true }) + + if (collectorRollers.isEmpty) { + driverHardware.driverJoystick.buttonPressed(JoystickButtons.Trigger) + .foreach(GearTasks.collectGear(gearTilter, gearRoller).toContinuous) + } } for { From eb6a7cba4e123b37496f4d4715caceb8eb8223a6 Mon Sep 17 00:00:00 2001 From: "Sang Hak (Andy) Chun" Date: Thu, 12 Oct 2017 21:51:59 -0700 Subject: [PATCH 07/11] Update camera URI to new Jetson URI (#96) --- 864-robot-config.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/864-robot-config.json b/864-robot-config.json index d37a88c..530f163 100644 --- a/864-robot-config.json +++ b/864-robot-config.json @@ -209,11 +209,11 @@ "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": { From 4a0d0b99f565651ab4d0f0d2b5c30043a7624478 Mon Sep 17 00:00:00 2001 From: Kunal Sheth Date: Fri, 13 Oct 2017 17:29:20 -0700 Subject: [PATCH 08/11] Fixed hardcoded ip in cam select --- .../seventeen/camselect/CamSelectHardware.scala | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 2e40edb30ecd9120137c13303833f51c0e6fe797 Mon Sep 17 00:00:00 2001 From: Shadaj Laddad Date: Sat, 14 Oct 2017 10:40:57 -0700 Subject: [PATCH 09/11] Changes for a working side gear autonomous --- 864-robot-config.json | 18 +- project/Versions.scala | 2 +- .../seventeen/AutoGenerator.scala | 29 +-- .../seventeen/CoreRobot.scala | 4 - .../seventeen/DefaultConfig.scala | 198 ++---------------- .../seventeen/LaunchRobot.scala | 6 +- .../seventeen/driver/DriverHardware.scala | 2 +- .../seventeen/drivetrain/package.scala | 7 +- 8 files changed, 52 insertions(+), 214 deletions(-) diff --git a/864-robot-config.json b/864-robot-config.json index 530f163..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": [ @@ -232,7 +232,7 @@ "collectorExtender": null, "collectorRollers": null, "gearRoller": { - "ports":{ + "ports": { "motor": 16 }, "props": { diff --git a/project/Versions.scala b/project/Versions.scala index 9c6f249..0ade535 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-27b86b3d" } \ 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 6daa6f3..5d282c9 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala @@ -51,10 +51,11 @@ class AutoGenerator(r: CoreRobot) { 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, gearRoller, gearTilter) ) @@ -62,20 +63,20 @@ class AutoGenerator(r: CoreRobot) { def rightGear(drivetrain: Drivetrain, gearRoller: GearRoller, gearTilter: GearTilter): FiniteTask = { new DriveDistanceStraight( - Inches(90.5), - Inches(3), - Degrees(10), - Percent(30) - )(drivetrain).withTimeout(Seconds(8)).then(new RotateByAngle( - Degrees(-57.61), + Inches(91.8 - 4 /* going too long in tests */), + Inches(1), Degrees(5), - 5 - )(drivetrain).withTimeout(Seconds(5))).then(new DriveDistanceStraight( - Inches(19.75), - Inches(3), + Percent(30) + )(drivetrain).withTimeout(Seconds(4)).then(new RotateByAngle( + Degrees(-60), + Degrees(1), + 20 + )(drivetrain).withTimeout(Seconds(4))).then(new DriveDistanceStraight( + Inches(19.75 + 3.5 /* for bumpers */ + 2 /* go in more */), + Inches(2), Degrees(10), Percent(30) - )(drivetrain).withTimeout(Seconds(5))).then( + )(drivetrain).withTimeout(Seconds(3))).then( toGearAndDrop(drivetrain, gearRoller, gearTilter) ) } @@ -83,7 +84,7 @@ class AutoGenerator(r: CoreRobot) { 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( diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala b/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala index 0303ce6..1cfe861 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/CoreRobot.scala @@ -315,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, diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala b/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala index 3e2929e..23b487a 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/DefaultConfig.scala @@ -207,26 +207,16 @@ object DefaultConfig { | ] | } | }, - | "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": { @@ -241,56 +231,28 @@ object DefaultConfig { | ] | } | }, - | "collectorElevator": { - | "port": { - | "motor": 1 - | }, - | "properties": { - | "collectSpeed": [ - | 100, - | "Percent$" - | ] - | } - | }, - | "collectorExtender": { - | "port": { - | "pneumatic": 3 - | } - | }, - | "collectorRollers": { - | "ports": { - | "rollerChannel": 0 - | }, - | "properties": { - | "lowRollerSpeedOutput": [ - | 0, - | "Percent$" - | ], - | "highRollerSpeedOutput": [ - | 100, - | "Percent$" - | ] - | } - | }, - | "gearRoller":{ + | "collectorElevator": null, + | "collectorExtender": null, + | "collectorRollers": null, + | "gearRoller": { | "ports":{ - | "motor":0 + | "motor": 16 | }, - | "props":{ - | "defaultHoldingPower":[ + | "props": { + | "defaultHoldingPower": [ | 10, | "Percent$" | ], - | "intakeGearPower":[ + | "intakeGearPower": [ | 100, | "Percent$" | ], - | "emitGearPower":[ + | "emitGearPower": [ | -50, | "Percent$" | ], - | "gearDetectionCurrent":[ - | 1000, + | "gearDetectionCurrent": [ + | 10, | "Amperes$" | ] | } @@ -300,130 +262,8 @@ object DefaultConfig { | "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, - | "Percent$" - | ], - | "speedTolerance": [ - | 75, - | "RevolutionsPerMinute$" - | ] - | } - | }, - | "shooterShifter": { - | "ports": { - | "pneumatic": 2 - | } - | }, - | "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 5bcf6e1..050ae93 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/LaunchRobot.scala @@ -59,8 +59,6 @@ class LaunchRobot extends RobotBase { implicit val clock = WPIClock - private implicit val hardware = RobotHardware(config.get) - private var coreRobot: CoreRobot = null private val ds = m_ds @@ -75,7 +73,9 @@ 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(latestConfigString), 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)) } } From 3d18bbc4f5fa37da7e0b2c73fe782197cd6d0255 Mon Sep 17 00:00:00 2001 From: Shadaj Laddad Date: Sat, 14 Oct 2017 12:01:20 -0700 Subject: [PATCH 10/11] Bump Potassium version --- project/Versions.scala | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/project/Versions.scala b/project/Versions.scala index 0ade535..b39d70c 100644 --- a/project/Versions.scala +++ b/project/Versions.scala @@ -1,3 +1,3 @@ object Versions { - val potassiumVersion = "0.1.0-27b86b3d" -} \ No newline at end of file + val potassiumVersion = "0.1.0-5d1a469a" +} From e2b4862f2e1fa876259ab5d566525502c907dfd9 Mon Sep 17 00:00:00 2001 From: Shadaj Laddad Date: Mon, 16 Oct 2017 11:23:22 -0700 Subject: [PATCH 11/11] Final changes for CalGames --- project/Versions.scala | 4 +-- .../seventeen/AutoGenerator.scala | 25 +++++++++++++------ 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/project/Versions.scala b/project/Versions.scala index b39d70c..7aba6fe 100644 --- a/project/Versions.scala +++ b/project/Versions.scala @@ -1,3 +1,3 @@ object Versions { - val potassiumVersion = "0.1.0-5d1a469a" -} + 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 5d282c9..b21cbba 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala +++ b/src/main/scala/com/lynbrookrobotics/seventeen/AutoGenerator.scala @@ -27,7 +27,7 @@ class AutoGenerator(r: CoreRobot) { private val gearPegDistance = Inches(109) - def slowCrossLine(drivetrain: Drivetrain): FiniteTask = { + def slowCrossLine(drivetrain: Drivetrain): FiniteTask = { new DriveDistanceStraight( Inches(107) - robotLength, Inches(3), @@ -42,7 +42,7 @@ class AutoGenerator(r: CoreRobot) { Inches(3), Degrees(10), Percent(30) - )(drivetrain)).withTimeout(Seconds(5)).andUntilDone( + )(drivetrain)).withTimeout(Seconds(2)).andUntilDone( GearTasks.scoreGear(gearTilter, gearRoller) ) @@ -67,18 +67,27 @@ class AutoGenerator(r: CoreRobot) { Inches(1), Degrees(5), Percent(30) - )(drivetrain).withTimeout(Seconds(4)).then(new RotateByAngle( - Degrees(-60), + )(drivetrain).withTimeout(Seconds(3)).then(new RotateByAngle( + Degrees(-58), Degrees(1), 20 - )(drivetrain).withTimeout(Seconds(4))).then(new DriveDistanceStraight( - Inches(19.75 + 3.5 /* for bumpers */ + 2 /* go in more */), + )(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(3))).then( + )(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(3))) } def leftGear(drivetrain: Drivetrain, gearRoller: GearRoller, gearTilter: GearTilter): FiniteTask = {