Skip to content

Commit

Permalink
finish wrist
Browse files Browse the repository at this point in the history
  • Loading branch information
@MatthewChoulas
MatthewChoulas committed Jan 21, 2024
1 parent 7d7dead commit c421304
Show file tree
Hide file tree
Showing 33 changed files with 2,890 additions and 2,486 deletions.
101 changes: 56 additions & 45 deletions src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,50 +12,61 @@ import org.team4099.lib.units.perSecond
import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6

class PositionVoltage(
private var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity
private var enableFOC: Boolean = true,
private var feedforward: ElectricalPotential = 0.0.volts,
private var slot: Int = 0,
private var overrideBrakeDurNeutral: Boolean = false,
private var limitForwardMotion: Boolean = false,
private var limitReverseMotion: Boolean = false,
private var velocity: AngularVelocity = 0.0.degrees.perSecond
private var position:
Angle, // Assuming an AngularPosition type exists similar to AngularVelocity
private var enableFOC: Boolean = true,
private var feedforward: ElectricalPotential = 0.0.volts,
private var slot: Int = 0,
private var overrideBrakeDurNeutral: Boolean = false,
private var limitForwardMotion: Boolean = false,
private var limitReverseMotion: Boolean = false,
private var velocity: AngularVelocity = 0.0.degrees.perSecond
) {

val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)

fun setPosition(new_position: Angle) {
position = new_position
positionVoltagePhoenix6.Position = new_position.inRotations
}

fun setEnableFOC(new_enableFOC: Boolean) {
enableFOC = new_enableFOC
positionVoltagePhoenix6.EnableFOC = new_enableFOC
}

fun setFeedforward(new_feedforward: ElectricalPotential) {
feedforward = new_feedforward
positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts
}

fun setSlot(new_slot: Int) {
slot = new_slot
positionVoltagePhoenix6.Slot = new_slot
}

fun setOverrideBrakeDurNeutral(new_override: Boolean) {
overrideBrakeDurNeutral = new_override
positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
}

fun setLimitForwardMotion(new_limitForward: Boolean) {
limitForwardMotion = new_limitForward
positionVoltagePhoenix6.LimitForwardMotion = new_limitForward
}

fun setLimitReverseMotion(new_limitReverse: Boolean) {
limitReverseMotion = new_limitReverse
positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse
}
}
val positionVoltagePhoenix6 =
PositionVoltagePhoenix6(
position.inRotations,
velocity.inRotationsPerSecond,
enableFOC,
feedforward.inVolts,
slot,
overrideBrakeDurNeutral,
limitForwardMotion,
limitReverseMotion
)

fun setPosition(new_position: Angle) {
position = new_position
positionVoltagePhoenix6.Position = new_position.inRotations
}

fun setEnableFOC(new_enableFOC: Boolean) {
enableFOC = new_enableFOC
positionVoltagePhoenix6.EnableFOC = new_enableFOC
}

fun setFeedforward(new_feedforward: ElectricalPotential) {
feedforward = new_feedforward
positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts
}

fun setSlot(new_slot: Int) {
slot = new_slot
positionVoltagePhoenix6.Slot = new_slot
}

fun setOverrideBrakeDurNeutral(new_override: Boolean) {
overrideBrakeDurNeutral = new_override
positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
}

fun setLimitForwardMotion(new_limitForward: Boolean) {
limitForwardMotion = new_limitForward
positionVoltagePhoenix6.LimitForwardMotion = new_limitForward
}

fun setLimitReverseMotion(new_limitReverse: Boolean) {
limitReverseMotion = new_limitReverse
positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse
}
}
98 changes: 55 additions & 43 deletions src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
package com.team4099.lib.phoenix6

import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6
import org.team4099.lib.units.AngularAcceleration
import org.team4099.lib.units.AngularVelocity
import org.team4099.lib.units.derived.ElectricalPotential
Expand All @@ -10,55 +9,68 @@ import org.team4099.lib.units.derived.volts
import org.team4099.lib.units.inRotationsPerSecond
import org.team4099.lib.units.inRotationsPerSecondPerSecond
import org.team4099.lib.units.perSecond
import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6

class VelocityVoltage(private var velocity: AngularVelocity,
private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond,
private var enableFOC:Boolean = true,
private var feedforward: ElectricalPotential = 0.0.volts,
private var slot:Int = 0,
private var overrideBrakeDurNeutral: Boolean = false,
private var limitForwardMotion: Boolean = false,
private var limitReverseMotion: Boolean = false){
class VelocityVoltage(
private var velocity: AngularVelocity,
private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond,
private var enableFOC: Boolean = true,
private var feedforward: ElectricalPotential = 0.0.volts,
private var slot: Int = 0,
private var overrideBrakeDurNeutral: Boolean = false,
private var limitForwardMotion: Boolean = false,
private var limitReverseMotion: Boolean = false
) {

val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
val velocityVoltagePhoenix6 =
VelocityVoltagePhoenix6(
velocity.inRotationsPerSecond,
acceleration.inRotationsPerSecondPerSecond,
enableFOC,
feedforward.inVolts,
slot,
overrideBrakeDurNeutral,
limitForwardMotion,
limitReverseMotion
)

fun setVelocity(new_velocity: AngularVelocity) {
velocity = new_velocity
velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond
}
fun setVelocity(new_velocity: AngularVelocity) {
velocity = new_velocity
velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond
}

fun setAcceleration(new_accel: AngularAcceleration) {
acceleration = new_accel
velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond
}
fun setAcceleration(new_accel: AngularAcceleration) {
acceleration = new_accel
velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond
}

fun setEnableFOC(new_enableFOC: Boolean) {
enableFOC = new_enableFOC
velocityVoltagePhoenix6.EnableFOC = new_enableFOC
}
fun setEnableFOC(new_enableFOC: Boolean) {
enableFOC = new_enableFOC
velocityVoltagePhoenix6.EnableFOC = new_enableFOC
}

fun setFeedforward(new_feedforward: ElectricalPotential) {
feedforward = new_feedforward
velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts
}
fun setFeedforward(new_feedforward: ElectricalPotential) {
feedforward = new_feedforward
velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts
}

fun setSlot(new_slot: Int) {
slot = new_slot
velocityVoltagePhoenix6.Slot = new_slot
}
fun setSlot(new_slot: Int) {
slot = new_slot
velocityVoltagePhoenix6.Slot = new_slot
}

fun setOverrideBrakeDurNeutral(new_override: Boolean) {
overrideBrakeDurNeutral = new_override
velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
}
fun setOverrideBrakeDurNeutral(new_override: Boolean) {
overrideBrakeDurNeutral = new_override
velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
}

fun setLimitForwardMotion(new_limitForward: Boolean) {
limitForwardMotion = new_limitForward
velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward
}
fun setLimitForwardMotion(new_limitForward: Boolean) {
limitForwardMotion = new_limitForward
velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward
}

fun setLimitReverseMotion(new_limitReverse: Boolean) {
limitReverseMotion = new_limitReverse
velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse
}
}
fun setLimitReverseMotion(new_limitReverse: Boolean) {
limitReverseMotion = new_limitReverse
velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse
}
}
10 changes: 5 additions & 5 deletions src/main/kotlin/com/team4099/robot2023/BuildConstants.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,10 @@ package com.team4099.robot2023
const val MAVEN_GROUP = ""
const val MAVEN_NAME = "Crescendo-2024"
const val VERSION = "unspecified"
const val GIT_REVISION = 83
const val GIT_SHA = "127888ea2c738b8d88ce875b8543d24eb247383f"
const val GIT_DATE = "2024-01-20T19:12:36Z"
const val GIT_REVISION = 85
const val GIT_SHA = "7d7deadf46b9a79f2de573bc014aadaf7438f346"
const val GIT_DATE = "2024-01-21T01:44:35Z"
const val GIT_BRANCH = "shooter"
const val BUILD_DATE = "2024-01-20T19:59:56Z"
const val BUILD_UNIX_TIME = 1705798796502L
const val BUILD_DATE = "2024-01-21T03:43:36Z"
const val BUILD_UNIX_TIME = 1705826616084L
const val DIRTY = 1
25 changes: 22 additions & 3 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,23 +9,29 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO
import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
import com.team4099.robot2023.subsystems.flywheel.Flywheel
import com.team4099.robot2023.subsystems.flywheel.FlywheelIOSim
import com.team4099.robot2023.subsystems.flywheel.FlywheelIOTalon
import com.team4099.robot2023.subsystems.limelight.LimelightVision
import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
import com.team4099.robot2023.subsystems.vision.Vision
import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
import com.team4099.robot2023.subsystems.wrist.Wrist
import com.team4099.robot2023.subsystems.wrist.WristIONeo
import com.team4099.robot2023.subsystems.wrist.WristIOSim
import com.team4099.robot2023.util.driver.Ryan
import edu.wpi.first.wpilibj.RobotBase
import org.team4099.lib.geometry.Pose2d
import org.team4099.lib.smoothDeadband
import org.team4099.lib.units.base.feet
import org.team4099.lib.units.derived.Angle
import org.team4099.lib.units.derived.degrees
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest

object RobotContainer {
private val drivetrain: Drivetrain
private val vision: Vision
private val limelight: LimelightVision
private val flywheel: Flywheel
private val wrist: Wrist

init {
if (RobotBase.isReal()) {
Expand All @@ -43,6 +49,9 @@ object RobotContainer {
// CameraIONorthstar("backward")
)
limelight = LimelightVision(object : LimelightVisionIO {})

flywheel = Flywheel(FlywheelIOTalon)
wrist = Wrist(WristIONeo)
} else {
// Simulation implementations
drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
Expand All @@ -53,6 +62,8 @@ object RobotContainer {
CameraIONorthstar("northstar_3"),
)
limelight = LimelightVision(object : LimelightVisionIO {})
flywheel = Flywheel(FlywheelIOSim)
wrist = Wrist(WristIOSim)
}

vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
Expand Down Expand Up @@ -129,6 +140,14 @@ object RobotContainer {
// Constants.Universal.Substation.SINGLE_SUBSTATION
// )
// )

ControlBoard.resetFlywheel.whileTrue(flywheel.flywheelResetCommand())
ControlBoard.spinUpFlywheel.whileTrue(flywheel.flywheelSpinUpCommand())
ControlBoard.openLoopFlywheel.whileTrue(flywheel.flywheelOpenLoopCommand())

ControlBoard.resetWrist.whileTrue(wrist.wristResetCommand())
ControlBoard.spinUpWrist.whileTrue(wrist.wristPositionCommand())
ControlBoard.openLoopWrist.whileTrue(wrist.wristOpenLoopCommand())
}

fun mapTestControls() {}
Expand Down
5 changes: 3 additions & 2 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,20 @@
package com.team4099.robot2023.commands.drivetrain

import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
import edu.wpi.first.wpilibj2.command.Command
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.derived.radians
import org.team4099.lib.units.perSecond
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest

class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() {
init {
addRequirements(drivetrain)
}

override fun execute() {
drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
drivetrain.currentRequest =
DrivetrainRequest.OpenLoop(
0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
)
drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) }
Expand Down
Loading

0 comments on commit c421304

Please sign in to comment.