Skip to content

Commit

Permalink
Merge pull request #24 from 5190GreenHopeRobotics/features/auto
Browse files Browse the repository at this point in the history 
Final Race Robot
  • Loading branch information
@ThePrimedTNT
ThePrimedTNT authored Feb 21, 2018
2 parents 15e7a95 + bf9e4a2 commit 782a07f
Show file tree
Hide file tree
Showing 107 changed files with 3,418 additions and 11,512 deletions.
2 changes: 1 addition & 1 deletion build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ plugins {
id "java"
id "eclipse"
id "idea"
id "jaci.openrio.gradle.GradleRIO" version "2018.01.22"
id "jaci.openrio.gradle.GradleRIO" version "2018.02.17"
}

def TEAM = 5190
Expand Down
39 changes: 20 additions & 19 deletions src/main/kotlin/frc/team5190/robot/Robot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,21 +5,21 @@

package frc.team5190.robot

import com.ctre.phoenix.motorcontrol.ControlMode
import edu.wpi.first.wpilibj.IterativeRobot
import edu.wpi.first.wpilibj.command.CommandGroup
import edu.wpi.first.wpilibj.command.Scheduler
import edu.wpi.first.wpilibj.livewindow.LiveWindow
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
import frc.team5190.robot.arm.ArmSubsystem
import frc.team5190.robot.auto.*
import frc.team5190.robot.auto.AutoHelper
import frc.team5190.robot.auto.StartingPositions
import frc.team5190.robot.drive.DriveSubsystem
import frc.team5190.robot.drive.Gear
import frc.team5190.robot.elevator.ElevatorSubsystem
import frc.team5190.robot.intake.IntakeSubsystem
import frc.team5190.robot.sensors.NavX
import frc.team5190.robot.util.Maths
import frc.team5190.robot.util.commandGroup
import frc.team5190.robot.vision.VisionSubsystem
import openrio.powerup.MatchData

Expand Down Expand Up @@ -65,11 +65,13 @@ class Robot : IterativeRobot() {
StartingPositions.values().forEach { sideChooser.addObject(it.name.toLowerCase().capitalize(), it) }
sideChooser.addDefault("Left", StartingPositions.LEFT)

SmartDashboard.putData("Side Selector", sideChooser)
SmartDashboard.putData("Starting Position", sideChooser)

controllerChooser.addObject("Xbox", "Xbox")
controllerChooser.addObject("Bongo", "Bongo")
controllerChooser.addDefault("Xbox", "Xbox")

SmartDashboard.putData("Controller", controllerChooser)
}

/**
Expand All @@ -88,19 +90,17 @@ class Robot : IterativeRobot() {
SmartDashboard.putNumber("Right Encoder to Feet", Maths.nativeUnitsToFeet(DriveSubsystem.falconDrive.rightEncoderPosition))

SmartDashboard.putNumber("Elevator Encoder Position", ElevatorSubsystem.currentPosition.toDouble())
SmartDashboard.putNumber("Elevator Inches Position", ElevatorSubsystem.nativeUnitsToInches(ElevatorSubsystem.currentPosition))

SmartDashboard.putNumber("Arm Encoder Position", ArmSubsystem.currentPosition.toDouble())

SmartDashboard.putNumber("Left Motor Amperage", DriveSubsystem.leftMotorAmperage)
SmartDashboard.putNumber("Right Motor Amerpage", DriveSubsystem.rightMotorAmperage)

SmartDashboard.putNumber("Arm Motor Amperage", ArmSubsystem.motorAmps)
SmartDashboard.putNumber("Elevator Motor Amperage", ElevatorSubsystem.motorCurrent)

SmartDashboard.putData("Elevator Subsystem", ElevatorSubsystem)
SmartDashboard.putData("Drive Subsystem", DriveSubsystem)
SmartDashboard.putData("Arm Subsystem", ArmSubsystem)
SmartDashboard.putData("Intake Subsystem", IntakeSubsystem)

SmartDashboard.putData("Gyro", NavX)

Scheduler.getInstance().run()
Expand All @@ -110,40 +110,41 @@ class Robot : IterativeRobot() {
* Executed when autonomous is initialized
*/
override fun autonomousInit() {
// ResetElevatorCommand().start()

DriveSubsystem.autoReset()
DriveSubsystem.falconDrive.gear = Gear.HIGH
NavX.reset()

this.pollForFMSData()
AutoHelper.getAuto(StartingPositions.CENTER, switchSide, scaleSide).start()
AutoHelper.getAuto(sideChooser.selected, switchSide, scaleSide).start()
}


override fun autonomousPeriodic() {}

/**
* Executed once when robot is disabled.
*/
override fun disabledInit() {
}
override fun disabledInit() {}

override fun disabledPeriodic() {
}
override fun disabledPeriodic() {}

/**
* Executed when teleop is initialized
*/
override fun teleopInit() {
// commandGroup {
// if (ArmSubsystem.currentPosition < ArmPosition.DOWN.ticks)
// addSequential(AutoArmCommand(ArmPosition.DOWN))
// addSequential(ResetElevatorCommand())
// }.start()

ElevatorSubsystem.set(ControlMode.MotionMagic, ElevatorSubsystem.currentPosition.toDouble())
ArmSubsystem.set(ControlMode.MotionMagic, ArmSubsystem.currentPosition.toDouble())

DriveSubsystem.currentCommand?.cancel()

DriveSubsystem.teleopReset()
DriveSubsystem.controller = controllerChooser.selected ?: "Xbox"
}

override fun teleopPeriodic() {}

private fun pollForFMSData() {
switchSide = MatchData.getOwnedSide(MatchData.GameFeature.SWITCH_NEAR)
scaleSide = MatchData.getOwnedSide(MatchData.GameFeature.SCALE)
Expand Down
6 changes: 3 additions & 3 deletions src/main/kotlin/frc/team5190/robot/arm/ArmSubsystem.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ object ArmSubsystem : Subsystem() {

private fun currentLimiting() {
currentBuffer.add(masterArmMotor.outputCurrent)
state = masterArmMotor.limitCurrent(currentBuffer)
state = limitCurrent(currentBuffer)

when (state) {
MotorState.OK -> {
Expand Down Expand Up @@ -81,8 +81,8 @@ object ArmSubsystem : Subsystem() {
}

enum class ArmPosition (val ticks: Int) {
BEHIND(ArmConstants.DOWN_TICKS + 1300),
BEHIND(ArmConstants.DOWN_TICKS + 1450),
UP(ArmConstants.DOWN_TICKS + 800),
MIDDLE(ArmConstants.DOWN_TICKS + 150),
MIDDLE(ArmConstants.DOWN_TICKS + 400),
DOWN(ArmConstants.DOWN_TICKS);
}
108 changes: 62 additions & 46 deletions src/main/kotlin/frc/team5190/robot/auto/AutoHelper.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ class AutoHelper {
return commandGroup {
this.addSequential(dropCubeOnScale(scale1Id, folder == "RS-LR", false))
this.addSequential(pickupCube(folder == "LS-RL"))
this.addSequential(switchToScale())
}
}
"LS-LR", "RS-RL" -> {
Expand All @@ -52,10 +53,10 @@ class AutoHelper {
})
this.addSequential(TurnCommand(-90.0, false))
this.addSequential(frc.team5190.robot.util.commandGroup {
this.addParallel(AutoArmCommand(frc.team5190.robot.arm.ArmPosition.DOWN))
this.addParallel(MotionMagicCommand(2.5), 1.0)
this.addParallel(AutoArmCommand(frc.team5190.robot.arm.ArmPosition.MIDDLE))
})
this.addSequential(IntakeCommand(IntakeDirection.OUT, timeout = 0.2, outSpeed = 0.5))
this.addSequential(IntakeCommand(IntakeDirection.OUT, timeout = 0.2, outSpeed = 0.4))
this.addSequential(IntakeHoldCommand(), 0.001)
}
}
Expand All @@ -70,59 +71,59 @@ class AutoHelper {
"CS-L" -> {
val switchId = Pathreader.requestPath("CS-L", "Switch")
val centerId = Pathreader.requestPath("CS-L", "Center")
val switch2Id = Pathreader.requestPath("CS-L", "Switch 2")
return commandGroup {
this.addSequential(commandGroup {
this.addParallel(MotionProfileCommand(switchId))
this.addParallel(AutoElevatorCommand(ElevatorPosition.SWITCH))
this.addParallel(AutoArmCommand(ArmPosition.MIDDLE))
})

this.addSequential(IntakeCommand(IntakeDirection.OUT, timeout = 0.2, outSpeed = 0.5))
this.addSequential(IntakeHoldCommand(), 0.001)

this.addSequential(MotionProfileCommand(centerId, true))
this.addSequential(dropCubeFromCenter(switchId))
this.addSequential(getBackToCenter(centerId))
this.addSequential(pickupCubeFromCenter())

this.addSequential(commandGroup {
this.addParallel(MotionProfileCommand(switchId))
})

this.addSequential(IntakeCommand(IntakeDirection.OUT, timeout = 0.2, outSpeed = 0.5))
this.addSequential(IntakeHoldCommand(), 0.001)

this.addSequential(dropCubeFromCenter(switch2Id))
this.addSequential((MotionMagicCommand(-2.00)))
}
}
"CS-R" -> {
val switchId = Pathreader.requestPath("CS-R", "Switch")
val centerId = Pathreader.requestPath("CS-R", "Center")
val switch2Id = Pathreader.requestPath("CS-R", "Switch 2")
return commandGroup {
this.addSequential(commandGroup {
this.addParallel(MotionProfileCommand(switchId))
this.addParallel(AutoElevatorCommand(ElevatorPosition.SWITCH))
this.addParallel(AutoArmCommand(ArmPosition.MIDDLE))
})

this.addSequential(IntakeCommand(IntakeDirection.OUT, timeout = 0.2, outSpeed = 0.5))
this.addSequential(IntakeHoldCommand(), 0.001)
this.addSequential(MotionProfileCommand(centerId, true))
this.addSequential(dropCubeFromCenter(switchId))
this.addSequential(getBackToCenter(centerId))
this.addSequential(pickupCubeFromCenter())
this.addSequential(dropCubeFromCenter(switch2Id))
this.addSequential((MotionMagicCommand(-2.00)))
}
}
else -> TODO("Does not exist.")
}
}

private fun switchToScale(): CommandGroup {
return commandGroup {
this.addSequential(commandGroup {
this.addParallel(AutoElevatorCommand(ElevatorPosition.SCALE))
this.addParallel(AutoArmCommand(ArmPosition.BEHIND))
this.addParallel(commandGroup {
this.addSequential(MotionMagicCommand(-4.5))
this.addSequential(TurnCommand(12.5))
})
})

this.addSequential(IntakeCommand(IntakeDirection.OUT, outSpeed = 1.0, timeout = 1.0))
this.addSequential(AutoArmCommand(frc.team5190.robot.arm.ArmPosition.MIDDLE))
}

}

private fun pickupCube(leftTurn: Boolean): CommandGroup {
return commandGroup {
this.addParallel(commandGroup {
this.addParallel(AutoElevatorCommand(ElevatorPosition.INTAKE))
this.addParallel(AutoArmCommand(ArmPosition.DOWN))
})
this.addParallel(commandGroup {
this.addSequential(TurnCommand(if (leftTurn) -10.0 else 10.0, visionCheck = true, tolerance = 15.0))
this.addSequential(TurnCommand(if (leftTurn) -10.0 else 5.0, visionCheck = true, tolerance = 10.0))
this.addSequential(commandGroup {
this.addParallel(MotionMagicCommand(4.0, cruiseVel = 3.0), 4.0 / 3.0)
this.addParallel(IntakeCommand(IntakeDirection.IN, timeout = 2.0))
this.addParallel(MotionMagicCommand(5.0, cruiseVel = 5.0))
this.addParallel(IntakeCommand(IntakeDirection.IN, timeout = 2.25, inSpeed = 0.75))
})
this.addSequential(IntakeHoldCommand(), 0.001)
})
Expand All @@ -146,8 +147,8 @@ class AutoHelper {
this.addSequential(AutoArmCommand(ArmPosition.BEHIND))
})
})
this.addSequential(TimedCommand(0.25))
this.addSequential(IntakeCommand(IntakeDirection.OUT, timeout = 1.0, outSpeed = 1.0))
// this.addSequential(TimedCommand(0.25)) // IS THIS NEEDED?
this.addSequential(IntakeCommand(IntakeDirection.OUT, timeout = 0.65, outSpeed = 0.65))
})
})
this.addSequential(IntakeHoldCommand(), 0.001)
Expand All @@ -156,36 +157,51 @@ class AutoHelper {

private fun dropCubeOnSwitch(): CommandGroup {
return commandGroup {
this.addSequential(MotionMagicCommand(-0.25))
this.addSequential(commandGroup {
this.addParallel(AutoElevatorCommand(ElevatorPosition.SWITCH))
this.addParallel(AutoArmCommand(ArmPosition.MIDDLE))
this.addParallel(commandGroup {
this.addSequential(TimedCommand(0.75))
this.addSequential(MotionMagicCommand(1.1), 1.0)
})
})
this.addSequential(MotionMagicCommand(1.3), 1.0)
this.addSequential(IntakeCommand(IntakeDirection.OUT, timeout = 0.2, outSpeed = 0.5))
this.addSequential(IntakeHoldCommand(), 0.001)
}
}

private fun pickupCubeFromCenter(): CommandGroup {
private fun dropCubeFromCenter(switchId: Int): CommandGroup {
return commandGroup {
this.addSequential(commandGroup {
this.addParallel(MotionProfileCommand(switchId))
this.addParallel(AutoElevatorCommand(ElevatorPosition.SWITCH))
this.addParallel(AutoArmCommand(ArmPosition.MIDDLE))
})

this.addSequential(IntakeCommand(IntakeDirection.OUT, timeout = 0.2, outSpeed = 0.5))
this.addSequential(IntakeHoldCommand(), 0.001)
}
}

private fun getBackToCenter(centerId: Int): CommandGroup {
return commandGroup {
this.addSequential(MotionProfileCommand(centerId, true))
this.addSequential(commandGroup {
this.addParallel(TurnCommand(0.0, true, 15.0))
this.addParallel(TurnCommand(0.0, false, 0.0))
this.addParallel(AutoElevatorCommand(ElevatorPosition.INTAKE))
this.addParallel(AutoArmCommand(ArmPosition.DOWN))
})
}
}

this.addSequential(frc.team5190.robot.util.commandGroup {
this.addParallel(MotionMagicCommand(3.75))
private fun pickupCubeFromCenter(): CommandGroup {
return commandGroup {
this.addSequential(commandGroup {
this.addParallel(MotionMagicCommand(4.00))
this.addParallel(IntakeCommand(IntakeDirection.IN, timeout = 2.0))
})

this.addSequential(IntakeHoldCommand(), 0.001)
this.addSequential(frc.team5190.robot.util.commandGroup {
this.addParallel(MotionMagicCommand(-4.0))
this.addParallel(AutoElevatorCommand(frc.team5190.robot.elevator.ElevatorPosition.SWITCH))
this.addParallel(AutoArmCommand(frc.team5190.robot.arm.ArmPosition.MIDDLE))
})
this.addSequential(MotionMagicCommand(-4.50))
}
}
}
Expand Down
7 changes: 3 additions & 4 deletions src/main/kotlin/frc/team5190/robot/auto/MotionMagicCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,10 @@ class MotionMagicCommand(feet: Double,
}

override fun end() {
DriveSubsystem.autoReset()
}
DriveSubsystem.falconDrive.leftMotors.forEach { it.inverted = false }
DriveSubsystem.falconDrive.rightMotors.forEach { it.inverted = true }

override fun execute() {
DriveSubsystem.falconDrive.feedSafety()
DriveSubsystem.falconDrive.tankDrive(ControlMode.PercentOutput, 0.0, 0.0)
}

override fun isFinished() = DriveSubsystem.falconDrive.allMasters.any {
Expand Down
1 change: 0 additions & 1 deletion src/main/kotlin/frc/team5190/robot/auto/MotionProfileCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,6 @@ class MotionProfileCommand(private val requestId: Int, private val isReversed: B
motionProfile.control()
DriveSubsystem.falconDrive.leftMaster.set(ControlMode.MotionProfile, motionProfile.getSetValue().value.toDouble())
DriveSubsystem.falconDrive.rightMaster.set(ControlMode.MotionProfile, motionProfile.getSetValue().value.toDouble())
DriveSubsystem.falconDrive.feedSafety()
}

/**
Expand Down
6 changes: 3 additions & 3 deletions src/main/kotlin/frc/team5190/robot/auto/TurnCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ import frc.team5190.robot.vision.VisionSubsystem
* Command that turns the robot to a certain angle
* @param angle Angle to turn to in degrees
*/
class TurnCommand(val angle: Double, val visionCheck: Boolean = false, val tolerance: Double = 0.0) : PIDCommand(0.075, 0.00, 0.1) {
class TurnCommand(val angle: Double, val visionCheck: Boolean = false, val tolerance: Double = 0.0) : PIDCommand(0.08, 0.001, 0.1) {

init {
requires(DriveSubsystem)
Expand All @@ -19,7 +19,7 @@ class TurnCommand(val angle: Double, val visionCheck: Boolean = false, val toler

override fun initialize() {
// Only execute the command for a total of a max of 5 seconds (should be close enough to target by then)
setTimeout(3.0)
setTimeout(2.5)
setName("DriveSystem", "RotateController")

when (visionCheck) {
Expand All @@ -32,7 +32,7 @@ class TurnCommand(val angle: Double, val visionCheck: Boolean = false, val toler
}
true -> {
val x = NavX.pidGet() // current absolute angle
val y = x + VisionSubsystem.tgtAngle // Vision absolute angle
val y = x + (VisionSubsystem.tgtAngle + VisionSubsystem.rawAngle) / 2.0 // Vision absolute angle
// (y - angle) is correction and it should be less than tolerance
setpoint = if (Math.abs(y - angle) < tolerance) {
println("Vision subsystem corrected $angle to $y")
Expand Down
Loading

0 comments on commit 782a07f

Please sign in to comment.