-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Updated Configs * Updated Configs * Changed PID and max speed * Changed PID and max speed * updated conversion factor * Update yagsl.json * Changed PID * Fixed Swerve * Added Corner Rotation Support --------- Co-authored-by: Micah Rao <[email protected]>
- Loading branch information
Showing
12 changed files
with
215 additions
and
146 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,20 +1,16 @@ | ||
| { | ||
| "wheelDiameter": 4, | ||
| "gearRatio": { | ||
| "drive": 8.14, | ||
| "angle": 21.42 | ||
| }, | ||
| "optimalVoltage": 12, | ||
| "wheelGripCoefficientOfFriction": 1.19, | ||
| "currentLimit": { | ||
| "drive": 40, | ||
| "angle": 20 | ||
| }, | ||
| "conversionFactor": { | ||
| "angle": 16.806722689075627, | ||
| "drive": 0.03921201641335663 | ||
| }, | ||
| "rampRate": { | ||
| "drive": 0.5, | ||
| "angle": 0.25 | ||
| }, | ||
| "encoderPulsePerRotation": { | ||
| "drive": 1, | ||
| "angle": 1 | ||
| }, | ||
| "wheelGripCoefficientOfFriction": 1.19 | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,6 +1,4 @@ | ||
| { | ||
| "maxSpeed": 12, | ||
| "optimalVoltage": 12, | ||
| "imu": { | ||
| "type": "pigeon2", | ||
| "id": 15, | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
56 changes: 56 additions & 0 deletions
56
src/main/java/frc/robot/commands/swerve/CornerSpinCommand.kt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,56 @@ | ||
| package frc.robot.commands.swerve | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.CommandBase | ||
| import SwerveSubsystem | ||
| import edu.wpi.first.math.geometry.Translation2d | ||
| import edu.wpi.first.math.util.Units | ||
| import frc.robot.util.SingletonXboxController | ||
| import swervelib.SwerveController | ||
| import java.util.function.BooleanSupplier | ||
| import java.util.function.DoubleSupplier | ||
|
|
||
| class CornerSpinCommand(rotation: DoubleSupplier, driveMode: BooleanSupplier, | ||
| slowMode: BooleanSupplier) : CommandBase() { | ||
| private val swerveSubsystem = SwerveSubsystem | ||
|
|
||
| private val rotation: DoubleSupplier | ||
|
|
||
| private val driveMode: BooleanSupplier | ||
| private val slowMode: BooleanSupplier | ||
|
|
||
| private val controller: SwerveController | ||
|
|
||
|
|
||
|
|
||
| init { | ||
| // each subsystem used by the command must be passed into the addRequirements() method | ||
| addRequirements(swerveSubsystem) | ||
| this.rotation = rotation | ||
| this.driveMode = driveMode | ||
| this.slowMode = slowMode | ||
|
|
||
| controller = swerveSubsystem.getSwerveController() | ||
| } | ||
|
|
||
| override fun initialize() {} | ||
|
|
||
| override fun execute() { | ||
| var rot = rotation.asDouble | ||
| val driveMode = driveMode.asBoolean | ||
|
|
||
| val center: Translation2d = SingletonXboxController.getPOVasCorner() | ||
|
|
||
| if(slowMode.asBoolean){ | ||
| rot *= 0.6 | ||
| } | ||
|
|
||
| swerveSubsystem.drive(Translation2d(0.0, 0.0), rot * controller.config.maxAngularVelocity, false, center) | ||
| } | ||
|
|
||
| override fun isFinished(): Boolean { | ||
| // TODO: Make this return true when this Command no longer needs to run execute() | ||
| return false | ||
| } | ||
|
|
||
| override fun end(interrupted: Boolean) {} | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,67 +1,82 @@ | ||
| package frc.robot.commands.swerve | ||
| // Copyright (c) FIRST and other WPILib contributors. | ||
| // Open Source Software; you can modify and/or share it under the terms of | ||
| // the WPILib BSD license file in the root directory of this project. | ||
| package frc.robot.commands.swervedrive.drivebase | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.CommandBase | ||
| import SwerveSubsystem | ||
| import SwerveSubsystem.drive | ||
| import SwerveSubsystem.getSwerveController | ||
| import edu.wpi.first.math.geometry.Translation2d | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard | ||
| import edu.wpi.first.wpilibj2.command.CommandBase | ||
| import swervelib.SwerveController | ||
| import java.util.function.BooleanSupplier | ||
| import java.util.function.DoubleSupplier | ||
| import kotlin.contracts.contract | ||
|
|
||
| class TeleopDrive(vForward: DoubleSupplier, vSide: DoubleSupplier, omega: DoubleSupplier, | ||
| driveMode: BooleanSupplier, isOpenLoop: Boolean, slowMode: BooleanSupplier) : CommandBase() { | ||
| private val swerveSubsystem = SwerveSubsystem | ||
|
|
||
| private val vForward: DoubleSupplier | ||
| private val vSide: DoubleSupplier | ||
| /** | ||
| * An example command that uses an example subsystem. | ||
| */ | ||
| class TeleopDrive( | ||
| vX: DoubleSupplier, vY: DoubleSupplier, omega: DoubleSupplier, | ||
| driveMode: BooleanSupplier, slowMode: BooleanSupplier | ||
| ) : CommandBase() { | ||
| private val vX: DoubleSupplier | ||
| private val vY: DoubleSupplier | ||
| private val omega: DoubleSupplier | ||
| private val driveMode: BooleanSupplier | ||
| private val isOpenLoop: Boolean | ||
| private val slowMode: BooleanSupplier | ||
|
|
||
| private val swerveController: SwerveController | ||
|
|
||
|
|
||
| private val controller: SwerveController | ||
| private val swerve: SwerveSubsystem | ||
|
|
||
| /** | ||
| * Creates a new ExampleCommand. | ||
| * | ||
| * @param swerve The subsystem used by this command. | ||
| */ | ||
| init { | ||
| // each subsystem used by the command must be passed into the addRequirements() method | ||
| addRequirements(swerveSubsystem) | ||
|
|
||
| this.vForward = vForward | ||
| this.vSide = vSide | ||
| this.swerve = SwerveSubsystem | ||
| this.vX = vX | ||
| this.vY = vY | ||
| this.omega = omega | ||
| this.driveMode = driveMode | ||
| this.isOpenLoop = isOpenLoop | ||
| this.slowMode = slowMode | ||
| this.swerveController = swerveSubsystem.getSwerveController() | ||
|
|
||
|
|
||
|
|
||
| controller = swerve.getSwerveController() | ||
| // Use addRequirements() here to declare subsystem dependencies. | ||
| addRequirements(swerve) | ||
| } | ||
|
|
||
| // Called when the command is initially scheduled. | ||
| override fun initialize() {} | ||
|
|
||
| // Called every time the scheduler runs while the command is scheduled. | ||
| override fun execute() { | ||
| var xVelocity = vForward.getAsDouble() | ||
| var yVelocity = vSide.getAsDouble() | ||
| var angleVelocity = omega.getAsDouble() | ||
|
|
||
| if(slowMode.asBoolean){ | ||
| var xVelocity = vX.asDouble | ||
| var yVelocity = vY.asDouble | ||
| var angVelocity = omega.asDouble | ||
| val slowMode = slowMode.asBoolean | ||
| SmartDashboard.putNumber("vX", xVelocity) | ||
| SmartDashboard.putNumber("vY", yVelocity) | ||
| SmartDashboard.putNumber("omega", angVelocity) | ||
|
|
||
| if(slowMode){ | ||
| xVelocity *= 0.6 | ||
| yVelocity *= 0.6 | ||
| angleVelocity *= 0.6 | ||
| angVelocity *= 0.6 | ||
| } | ||
|
|
||
| var speed: Translation2d = Translation2d(xVelocity * swerveController.config.maxSpeed , yVelocity * swerveController.config.maxSpeed) | ||
|
|
||
| swerveSubsystem.drive(speed, angleVelocity * swerveController.config.maxAngularVelocity, driveMode.asBoolean) | ||
|
|
||
|
|
||
| // Drive using raw values. | ||
| swerve.drive( | ||
| Translation2d(xVelocity * swerve.maximumSpeed, yVelocity * swerve.maximumSpeed), | ||
| angVelocity * controller.config.maxAngularVelocity, | ||
| driveMode.asBoolean | ||
| ) | ||
| } | ||
|
|
||
| // Called once the command ends or is interrupted. | ||
| override fun end(interrupted: Boolean) {} | ||
|
|
||
| // Returns true when the command should end. | ||
| override fun isFinished(): Boolean { | ||
| // TODO: Make this return true when this Command no longer needs to run execute() | ||
| return false | ||
| } | ||
|
|
||
| override fun end(interrupted: Boolean) {} | ||
| } | ||
| } |
Oops, something went wrong.