Skip to content

Commit

Permalink
auto tuning 3/13
Browse files Browse the repository at this point in the history
  • Loading branch information
Shom770 committed Mar 14, 2024
1 parent 03eff2c commit dc2df65
Show file tree
Hide file tree
Showing 6 changed files with 60 additions and 31 deletions.
39 changes: 18 additions & 21 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package com.team4099.robot2023

import WheelRadiusCharacterizationCommand
import com.team4099.robot2023.auto.AutonomousSelector
import com.team4099.robot2023.commands.characterization.FeedForwardCharacterizationCommand
import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
Expand Down Expand Up @@ -216,22 +217,22 @@ object RobotContainer {
// else -60.degrees
// )
// )

ControlBoard.climbAlignRight.whileTrue(
TargetAngleCommand(
driver = Ryan(),
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
{ ControlBoard.slowMode },
drivetrain,
if (DriverStation.getAlliance().isPresent &&
DriverStation.getAlliance().get() == DriverStation.Alliance.Red
)
-120.0.degrees
else 60.0.degrees,
)
)
//
// ControlBoard.climbAlignRight.whileTrue(
// TargetAngleCommand(
// driver = Ryan(),
// { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
// { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
// { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
// { ControlBoard.slowMode },
// drivetrain,
// if (DriverStation.getAlliance().isPresent &&
// DriverStation.getAlliance().get() == DriverStation.Alliance.Red
// )
// -120.0.degrees
// else 60.0.degrees,
// )
// )

ControlBoard.targetSpeaker.whileTrue(
ParallelCommandGroup(
Expand All @@ -249,11 +250,7 @@ object RobotContainer {
)

ControlBoard.characterizeSubsystem.whileTrue(
FeedForwardCharacterizationCommand(
drivetrain,
{ voltage -> drivetrain.currentRequest = DrivetrainRequest.Characterize(voltage) },
{ drivetrain.fieldVelocity.magnitude }
)
WheelRadiusCharacterizationCommand(drivetrain, WheelRadiusCharacterizationCommand.Companion.Direction.CLOCKWISE)
)

/*
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ object AutonomousSelector {
get() = secondaryWaitInAuto.getDouble(0.0).seconds

fun getCommand(drivetrain: Drivetrain, superstructure: Superstructure): Command {
val mode = autonomousModeChooser.get()
val mode = AutonomousMode.TEST_AUTO_PATH

when (mode) {
AutonomousMode.TEST_AUTO_PATH ->
Expand Down
Original file line number Diff line number Diff line change
@@ -1,32 +1,39 @@
package com.team4099.robot2023.commands.characterization

import com.team4099.robot2023.config.constants.DrivetrainConstants
import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal
import com.team4099.utils.PolynomialRegression
import edu.wpi.first.wpilibj.Timer
import edu.wpi.first.wpilibj2.command.Command
import edu.wpi.first.wpilibj2.command.SubsystemBase
import org.littletonrobotics.junction.Logger
import org.team4099.lib.units.LinearVelocity
import org.team4099.lib.units.base.inSeconds
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.base.seconds
import org.team4099.lib.units.derived.ElectricalPotential
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inVolts
import org.team4099.lib.units.derived.volts
import org.team4099.lib.units.inMetersPerSecond
import org.team4099.lib.units.perSecond
import java.util.LinkedList
import java.util.function.Consumer
import java.util.function.Supplier

class FeedForwardCharacterizationCommand(
val subsystem: SubsystemBase,
val drivetrain: Drivetrain,
val voltageConsumer: Consumer<ElectricalPotential>,
val velocitySupplier: Supplier<LinearVelocity>
) : Command() {
private val startDelay = 2.0.seconds
private val startDelay = 1.0.seconds
private val rampRatePerSecond = 0.1.volts
private val timer = Timer()
private lateinit var data: FeedForwardCharacterizationData

init {
addRequirements(subsystem)
addRequirements(drivetrain)
}

override fun initialize() {
Expand All @@ -36,16 +43,29 @@ class FeedForwardCharacterizationCommand(
}

override fun execute() {
drivetrain.swerveModules.forEach { it.zeroSteering() }

if (timer.get() < startDelay.inSeconds) {
voltageConsumer.accept(0.volts)
}
else {
val voltage = ((timer.get() - startDelay.inSeconds) * rampRatePerSecond.inVolts).volts
voltageConsumer.accept(voltage)
data.add(velocitySupplier.get(), voltage)
Logger.recordOutput("Drivetrain/appliedVoltage", voltage.inVolts)
}
}

override fun end(interrupted: Boolean) {
voltageConsumer.accept(0.volts)
timer.stop()
data.print()
}

override fun isFinished(): Boolean {
return false
}

companion object {
class FeedForwardCharacterizationData {
private val velocityData: MutableList<LinearVelocity> = LinkedList()
Expand All @@ -70,7 +90,9 @@ class FeedForwardCharacterizationCommand(
println("\tCount=" + Integer.toString(velocityData.size) + "")
println(String.format("\tR2=%.5f", regression.R2()))
println(String.format("\tkS=%.5f", regression.beta(0)))
Logger.recordOutput("Drivetrain/kSFound", regression.beta(0))
println(String.format("\tkV=%.5f", regression.beta(1)))
Logger.recordOutput("Drivetrain/kVFound", regression.beta(1))
}
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -101,12 +101,16 @@ class WheelRadiusCharacterizationCommand(

override fun end(interrupted: Boolean) {
if (accumGyroYawRads <= (Math.PI * 2.0).radians) {
println("Not enough data for characterization")
Logger.recordOutput("Drivetrain/radiansOffFromWheelRadius", ((Math.PI * 2.0).radians - accumGyroYawRads).inRadians)
} else {
println("Effective Wheel Radius: ${currentEffectiveWheelRadius.inInches} inches")
Logger.recordOutput("Drivetrain/effectiveWheelRadius", currentEffectiveWheelRadius.inInches)
}
}

override fun isFinished(): Boolean {
return false
}

companion object {
enum class Direction(val value: Int) {
CLOCKWISE(-1),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ object DrivetrainConstants {
const val OMOMETRY_UPDATE_FREQUENCY = 250.0

const val WHEEL_COUNT = 4
val WHEEL_DIAMETER = 3.827.inches
val WHEEL_DIAMETER = (2.083 * 2).inches
val DRIVETRAIN_LENGTH = 22.750.inches
val DRIVETRAIN_WIDTH = 22.750.inches

Expand Down Expand Up @@ -158,8 +158,8 @@ object DrivetrainConstants {

val DRIVE_KFF = 12.0.volts / 4.1675.meters.perSecond

val DRIVE_KS = 0.5.volts
val DRIVE_KV = 0.145.volts / 1.0.meters.perSecond
val DRIVE_KS = 0.177.volts
val DRIVE_KV = 0.12.volts / 1.0.meters.perSecond
val DRIVE_KA = 0.0.volts / 1.0.meters.perSecond.perSecond

// val DRIVE_KS = 0.23677.volts
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@ import com.ctre.phoenix6.configs.Slot0Configs
import com.ctre.phoenix6.configs.TalonFXConfiguration
import com.ctre.phoenix6.controls.DutyCycleOut
import com.ctre.phoenix6.controls.PositionDutyCycle
import com.ctre.phoenix6.controls.TorqueCurrentFOC
import com.ctre.phoenix6.controls.VelocityVoltage
import com.ctre.phoenix6.controls.VoltageOut
import com.ctre.phoenix6.hardware.TalonFX
Expand Down Expand Up @@ -432,6 +433,11 @@ class SwerveModuleIOTalon(
}

override fun runCharacterization(input: ElectricalPotential) {
driveFalcon.setControl(VoltageOut(input.inVolts))
if (label == Constants.Drivetrain.FRONT_LEFT_MODULE_NAME) {
driveFalcon.setControl(DutyCycleOut(-input.inVolts / 12.0))
}
else {
driveFalcon.setControl(DutyCycleOut(input.inVolts / 12.0))
}
}
}

0 comments on commit dc2df65

Please sign in to comment.