Skip to content

Commit

Permalink
fixed some programming issues
Browse files Browse the repository at this point in the history
nbhog committed Jan 21, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
1 parent 1082757 commit 4d50bec
Showing 27 changed files with 2,389 additions and 2,211 deletions.
101 changes: 56 additions & 45 deletions src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt
Original file line number Diff line number Diff line change
@@ -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,
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,
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
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
@@ -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
}
}
5 changes: 1 addition & 4 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
Original file line number Diff line number Diff line change
@@ -14,16 +14,14 @@ import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
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.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
@@ -140,7 +138,6 @@ object RobotContainer {
ControlBoard.elevatorOpenLoopRetract.whileTrue(elevator.testElevatorOpenLoopRetractCommand())
ControlBoard.elevatorClosedLoopHigh.whileTrue(elevator.testElevatorClosedLoopExtendCommand())
ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.testElevatorClosedLoopExtendCommand())

}

fun mapTestControls() {}
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) }
Original file line number Diff line number Diff line change
@@ -6,7 +6,6 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator
import com.team4099.lib.trajectory.Waypoint
import com.team4099.robot2023.config.constants.DrivetrainConstants
import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
import com.team4099.robot2023.util.AllianceFlipUtil
import com.team4099.robot2023.util.Velocity2d
import edu.wpi.first.math.kinematics.ChassisSpeeds
@@ -22,8 +21,6 @@ import org.team4099.lib.geometry.Pose2d
import org.team4099.lib.geometry.Pose2dWPILIB
import org.team4099.lib.hal.Clock
import org.team4099.lib.kinematics.ChassisAccels
import java.util.function.Supplier
import kotlin.math.PI
import org.team4099.lib.units.Velocity
import org.team4099.lib.units.base.Meter
import org.team4099.lib.units.base.inMeters
@@ -55,6 +52,9 @@ import org.team4099.lib.units.inMetersPerSecondPerSecond
import org.team4099.lib.units.inRadiansPerSecond
import org.team4099.lib.units.inRadiansPerSecondPerSecond
import org.team4099.lib.units.perSecond
import java.util.function.Supplier
import kotlin.math.PI
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest

class DrivePathCommand(
val drivetrain: Drivetrain,
@@ -244,7 +244,8 @@ class DrivePathCommand(
Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)
)

drivetrain.currentRequest = DrivetrainRequest.ClosedLoop(
drivetrain.currentRequest =
DrivetrainRequest.ClosedLoop(
nextDriveState,
ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
)
@@ -309,16 +310,18 @@ class DrivePathCommand(
Logger.recordOutput("ActiveCommands/DrivePathCommand", false)
if (interrupted) {
// Stop where we are if interrupted
drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
)
drivetrain.currentRequest =
DrivetrainRequest.OpenLoop(
0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
)
} else {
// Execute one last time to end up in the final state of the trajectory
// Since we weren't interrupted, we know curTime > endTime
execute()
drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
)
drivetrain.currentRequest =
DrivetrainRequest.OpenLoop(
0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
)
}
}
}
Original file line number Diff line number Diff line change
@@ -3,18 +3,28 @@ package com.team4099.robot2023.commands.drivetrain
import com.team4099.lib.logging.LoggedTunableValue
import com.team4099.robot2023.config.constants.DrivetrainConstants
import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
import edu.wpi.first.wpilibj.RobotBase
import edu.wpi.first.wpilibj2.command.Command
import org.littletonrobotics.junction.Logger
import org.team4099.lib.controller.ProfiledPIDController
import org.team4099.lib.controller.TrapezoidProfile
import org.team4099.lib.units.Velocity
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.derived.*
import org.team4099.lib.units.derived.Angle
import org.team4099.lib.units.derived.Radian
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inDegrees
import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree
import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond
import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds
import org.team4099.lib.units.derived.perDegree
import org.team4099.lib.units.derived.perDegreePerSecond
import org.team4099.lib.units.derived.perDegreeSeconds
import org.team4099.lib.units.derived.radians
import org.team4099.lib.units.inDegreesPerSecond
import org.team4099.lib.units.perSecond
import kotlin.math.PI
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest

class GoToAngle(val drivetrain: Drivetrain) : Command() {
private val thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
@@ -82,7 +92,8 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() {

val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle)

drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
drivetrain.currentRequest =
DrivetrainRequest.OpenLoop(
thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true
)

@@ -98,8 +109,9 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() {
}

override fun end(interrupted: Boolean) {
drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
)
drivetrain.currentRequest =
DrivetrainRequest.OpenLoop(
0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
)
}
}
Loading

0 comments on commit 4d50bec

Please sign in to comment.