Skip to content

Add Elevator to Master (example) plz no consider #21

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
rbhog wants to merge 1 commit into from
Closed

Add Elevator to Master (example) plz no consider #21

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/main/java/org/usfirst/frc/team4099/robot/Constants.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,11 +119,11 @@ class Constants {
}

object Elevator {
val ELEVATOR_TALON_ID = 10
val ELEVATOR_TALON_ID = 0
}

object Wrist {
val WRIST_TALON_ID = 12
val WRIST_TALON_ID = 1

val WRIST_KP = 0.0000
val WRIST_KI = 0.0000
Expand Down
9 changes: 3 additions & 6 deletions src/main/java/org/usfirst/frc/team4099/robot/ControlBoard.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,19 +1,16 @@
package org.usfirst.frc.team4099.robot

import org.usfirst.frc.team4099.lib.joystick.DualShock4Gamepad
import org.usfirst.frc.team4099.lib.joystick.Gamepad
import org.usfirst.frc.team4099.lib.joystick.JoystickUtils
import org.usfirst.frc.team4099.lib.joystick.XboxOneGamepad
import org.usfirst.frc.team4099.lib.joystick.*

class ControlBoard private constructor() {
private val driver: Gamepad = XboxOneGamepad(Constants.Joysticks.DRIVER_PORT)
private val operator: Gamepad = XboxOneGamepad(Constants.Joysticks.SHOTGUN_PORT)
private val operator: Gamepad = LogitechF310Gamepad(Constants.Joysticks.SHOTGUN_PORT)

val throttle: Double
get() = driver.rightTriggerAxis - driver.leftTriggerAxis

val turn: Double
get() = driver.leftXAxis
get() = -driver.leftXAxis

val switchToHighGear: Boolean
get() = driver.rightShoulderButton
Expand Down
88 changes: 50 additions & 38 deletions src/main/java/org/usfirst/frc/team4099/robot/Robot.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package org.usfirst.frc.team4099.robot

import edu.wpi.first.wpilibj.IterativeRobot
import edu.wpi.first.wpilibj.Timer
import edu.wpi.first.wpilibj.livewindow.LiveWindow
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
import org.usfirst.frc.team4099.auto.AutoModeExecuter
Expand Down Expand Up @@ -55,10 +56,10 @@ class Robot : IterativeRobot() {

//TODO: add the robot state estimator here
// CameraServer.getInstance().startAutomaticCapture()
// enabledLooper.register(drive.loop)
enabledLooper.register(intake.loop)
enabledLooper.register(elevator.loop)
enabledLooper.register(wrist.loop)
enabledLooper.register(drive.loop)
// enabledLooper.register(intake.loop)
// enabledLooper.register(elevator.loop)
// enabledLooper.register(wrist.loop)

enabledLooper.register(BrownoutDefender.instance)

Expand Down Expand Up @@ -165,45 +166,45 @@ class Robot : IterativeRobot() {
SmartDashboard.putBoolean("isQuickTurn", isQuickTurn)
SmartDashboard.putNumber("voltage", VoltageEstimator.instance.averageVoltage)

if (controls.test) {
println("testing")
drive.setVelocitySetpoint(600 * throttle, 600 * throttle)
} else {
if (drive.highGear && shiftToLowGear) {
drive.highGear = false
println("Shifting to low gear")
} else if (!drive.highGear && shiftToHighGear) {
drive.highGear = true
println("Shifting to high gear")
}
if (intake.open && closeIntake) {
intake.open = false
println("Closing intake")
} else if (!intake.open && openIntake) {
intake.open = true
println("Opening intake")
}
// drive.setOpenLoop(cheesyDriveHelper.curvatureDrive(throttle, turn, isQuickTurn))
}

if (reverseIntake) {
intake.intakeState = Intake.IntakeState.OUT
} else if (intake.intakeState != Intake.IntakeState.SLOW) {
intake.intakeState = Intake.IntakeState.IN
}
intake.intakeState = Intake.IntakeState.STOP
// if (controls.test) {
// println("testing")
// drive.setVelocitySetpoint(600 * throttle, 600 * throttle)
// } else {
// if (drive.highGear && shiftToLowGear) {
// drive.highGear = false
// println("Shifting to low gear")
// } else if (!drive.highGear && shiftToHighGear) {
// drive.highGear = true
// println("Shifting to high gear")
// }
// if (intake.open && closeIntake) {
// intake.open = false
// println("Closing intake")
// } else if (!intake.open && openIntake) {
// intake.open = true
// println("Opening intake")
// }
println(throttle)
drive.setOpenLoop(cheesyDriveHelper.curvatureDrive(throttle, turn, isQuickTurn))
// }

// if (reverseIntake) {
// intake.intakeState = Intake.IntakeState.OUT
// } else if (intake.intakeState != Intake.IntakeState.SLOW) {
// intake.intakeState = Intake.IntakeState.IN
// }
//
when {
controls.elevatorTop -> elevator.elevatorState = Elevator.ElevatorState.HIGH
controls.elevatorBottom -> elevator.elevatorState = Elevator.ElevatorState.LOW
else -> elevator.setOpenLoop(controls.elevatorPower)
}

when {
controls.wristTop -> wrist.wristState = Wrist.WristState.STOWED_UP
controls.wristBottom -> wrist.wristState = Wrist.WristState.HORIZONTAL
else -> wrist.setOpenLoop(controls.wristPower)
}
// when {
// controls.wristTop -> wrist.wristState = Wrist.WristState.STOWED_UP
// controls.wristBottom -> wrist.wristState = Wrist.WristState.HORIZONTAL
// else -> wrist.setOpenLoop(controls.wristPower)
// }

signalTable.sensorPosition = elevator.talon.sensorCollection.quadraturePosition
signalTable.sensorVelocity = elevator.talon.sensorCollection.quadratureVelocity
Expand All @@ -214,7 +215,10 @@ class Robot : IterativeRobot() {
signalTable.elevatorCurrent = elevator.talon.outputCurrent
csvWriter.add(signalTable)

println(wrist.talon.sensorCollection.quadraturePosition)
// println(wrist.talon.sensorCollection.quadraturePosition)
println("Current: ${elevator.talon.outputCurrent}")
println("Voltage: ${elevator.talon.motorOutputVoltage}")
println("Output: ${elevator.talon.motorOutputPercent}")

outputAllToSmartDashboard()
updateDashboardFeedback() // things such as is aligned?, etc
Expand Down Expand Up @@ -268,7 +272,15 @@ class Robot : IterativeRobot() {
//
// }

override fun testPeriodic() = teleopPeriodic()
// override fun testPeriodic() = teleopPeriodic()

override fun testPeriodic() {
val initial = Timer.getFPGATimestamp()
while (Timer.getFPGATimestamp() - initial < 300) {
drive.setOpenLoop(DriveSignal(1.0, 1.0))
}
drive.setOpenLoop(DriveSignal(0.0, 0.0))
}

/**
* Log information from all subsystems onto the SmartDashboard
Expand Down