Skip to content

Commit

Permalink
Tuning for autonns
Browse files Browse the repository at this point in the history
  • Loading branch information
@anivanchen
anivanchen committed Jan 27, 2024
1 parent 6856802 commit fbfb2fc
Show file tree
Hide file tree
Showing 12 changed files with 73 additions and 50 deletions.
6 changes: 3 additions & 3 deletions .pathplanner/settings.json
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
{
"robotWidth": 0.66,
"robotLength": 0.66,
"robotWidth": 0.69,
"robotLength": 0.81,
"holonomicMode": true,
"pathFolders": [
"ABCDE",
"HFE"
],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAccel": 1.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"maxModuleSpeed": 4.5
Expand Down
8 changes: 1 addition & 7 deletions src/main/deploy/pathplanner/autos/4 Piece ABC.auto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
"version": 1.0,
"startingPose": {
"position": {
"x": 1.41,
"x": 1.5,
"y": 4.3
},
"rotation": 0.0
Expand All @@ -11,12 +11,6 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intake acquire"
}
},
{
"type": "path",
"data": {
Expand Down
20 changes: 10 additions & 10 deletions src/main/deploy/pathplanner/paths/B To A (ABC).path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 2.75,
"y": 5.64
"x": 2.45,
"y": 5.54
},
"prevControl": null,
"nextControl": {
"x": 2.316987298107781,
"y": 5.89
"x": 2.0917768750323384,
"y": 5.9449473513024005
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.75,
"y": 6.88
"x": 2.56343173023441,
"y": 6.802894337993537
},
"prevControl": {
"x": 2.316987298107781,
"y": 6.63
"x": 2.1304190283421907,
"y": 6.552894337993537
},
"nextControl": null,
"isLocked": false,
Expand All @@ -33,7 +33,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -45,5 +45,5 @@
"reversed": false,
"folder": "ABCDE",
"previewStartingState": null,
"useDefaultConstraints": false
"useDefaultConstraints": true
}
18 changes: 9 additions & 9 deletions src/main/deploy/pathplanner/paths/C To B (ABC).path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 2.6,
"x": 2.45,
"y": 4.3
},
"prevControl": null,
"nextControl": {
"x": 2.0254666676607664,
"y": 4.782090707264905
"x": 2.0811396483180697,
"y": 4.8150477496701525
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.75,
"y": 5.64
"x": 2.45,
"y": 5.54
},
"prevControl": {
"x": 2.257596123493896,
"y": 5.553175911166535
"x": 2.0964466094067262,
"y": 5.186446609406726
},
"nextControl": null,
"isLocked": false,
Expand All @@ -33,7 +33,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -45,5 +45,5 @@
"reversed": false,
"folder": "ABCDE",
"previewStartingState": null,
"useDefaultConstraints": false
"useDefaultConstraints": true
}
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/Start To C (ABC).path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,11 @@
},
{
"anchor": {
"x": 2.6,
"x": 2.45,
"y": 4.3
},
"prevControl": {
"x": 2.1000000000000005,
"x": 1.9500000000000006,
"y": 4.3
},
"nextControl": null,
Expand All @@ -33,7 +33,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
Expand All @@ -48,5 +48,5 @@
"rotation": 0.0,
"velocity": 0
},
"useDefaultConstraints": false
"useDefaultConstraints": true
}
2 changes: 1 addition & 1 deletion src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;

public class SwerveDriveDrive extends Command {
Expand Down Expand Up @@ -102,6 +103,5 @@ public void execute() {
else {
swerve.drive(speed.get(), angularVel);
}

}
}
1 change: 1 addition & 0 deletions src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToScore.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,7 @@ private void updateTarget() {
Vector2D targetPos = speakerPos.add(robotPos.sub(speakerPos).normalize().mul(Units.inchesToMeters(TARGET_DISTANCE_IN.get())));
this.target = new Pose2d(targetPos.x, targetPos.y, new Rotation2d(Math.atan2(targetPos.y - robot.getY(), targetPos.x - robot.getX())));
this.distance = targetPos.sub(robotPos).magnitude();

}

@Override
Expand Down
6 changes: 5 additions & 1 deletion src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToScore.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,14 +46,18 @@ public void initialize() {
Vector2D speakerPos = new Vector2D(speaker.getX(), speaker.getY());
Vector2D robotPos = new Vector2D(robot.getX(), robot.getY());
Vector2D targetPos = speakerPos.add(robotPos.sub(speakerPos).normalize().mul(Units.inchesToMeters(TARGET_DISTANCE_IN.get())));
this.target = new Pose2d(targetPos.x, targetPos.y, new Rotation2d(Math.atan2(targetPos.y - robot.getY(), targetPos.x - robot.getX())));
// this.target = new Pose2d(targetPos.x, targetPos.y, new Rotation2d(Math.atan2(targetPos.y - robot.getY(), targetPos.x - robot.getX())));
Rotation2d targetAngle = targetPos.getTranslation2d().minus(robotPos.getTranslation2d()).getAngle().plus(Rotation2d.fromDegrees(180));
this.target = new Pose2d(targetPos.x, targetPos.y, targetAngle);
SmartDashboard.putNumber("Vision/To Score/Target Angle", targetAngle.getDegrees());
}

@Override
public void execute() {
controller.update(target, AbstractOdometry.getInstance().getPose());
swerve.setChassisSpeeds(controller.getOutput());
targetPose2d.setPose(target);

}

@Override
Expand Down
26 changes: 19 additions & 7 deletions src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
package com.stuypulse.robot.commands.swerve;

import static com.stuypulse.robot.constants.Settings.Alignment.TARGET_DISTANCE_IN;

import com.stuypulse.robot.constants.Field;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.constants.Settings.Alignment;
import com.stuypulse.robot.constants.Settings.Driver.Drive;
Expand All @@ -9,21 +12,24 @@
import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
import com.stuypulse.stuylib.input.Gamepad;
import com.stuypulse.stuylib.math.Angle;
import com.stuypulse.stuylib.math.Vector2D;
import com.stuypulse.stuylib.streams.vectors.VStream;
import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone;
import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;

public class SwerveDriveWithAiming extends Command {

private final SwerveDrive swerve;

private VStream speed;
private final AngleController gyroFeedback;
private final AngleController angleController;
private final Pose2d target;

public SwerveDriveWithAiming(Pose2d target, Gamepad driver) {
Expand All @@ -39,26 +45,32 @@ public SwerveDriveWithAiming(Pose2d target, Gamepad driver) {
new VLowPassFilter(Drive.RC)
);

this.gyroFeedback = new AnglePIDController(Alignment.Gyro.P.get(), Alignment.Gyro.I.get(), Alignment.Gyro.D.get());
this.angleController = new AnglePIDController(Alignment.Rotation.P, Alignment.Rotation.I, Alignment.Rotation.D);
this.target = target;

addRequirements(swerve);
}

@Override
public void execute() {

Pose2d robotPose = AbstractOdometry.getInstance().getPose();

Rotation2d target = new Rotation2d(
robotPose.getX() - this.target.getX(),
robotPose.getY() - this.target.getY())
.minus(Rotation2d.fromDegrees(180));
robotPose.getY() - this.target.getY());

double angularVel = -gyroFeedback.update(
Angle.fromRotation2d(target.plus(Rotation2d.fromDegrees(180))),
double angularVel = -angleController.update(
Angle.fromRotation2d(target),
Angle.fromRotation2d(swerve.getGyroAngle()));

Pose2d speaker = Field.getSpeakerPose();
Pose2d robot = AbstractOdometry.getInstance().getPose();
Vector2D speakerPos = new Vector2D(speaker.getX(), speaker.getY());
Vector2D robotPos = new Vector2D(robot.getX(), robot.getY());
Vector2D targetPos = speakerPos.add(robotPos.sub(speakerPos).normalize().mul(Units.inchesToMeters(TARGET_DISTANCE_IN.get())));
Rotation2d targetAngle = targetPos.getTranslation2d().minus(robotPos.getTranslation2d()).getAngle().plus(Rotation2d.fromDegrees(180));
SmartDashboard.putNumber("Vision/To Score/Target Angle", targetAngle.getDegrees());

swerve.drive(speed.get(), angularVel);
}
}
2 changes: 1 addition & 1 deletion src/main/java/com/stuypulse/robot/constants/Field.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ public static boolean isValidFiducialID(int id) {
}

Pose2d SPEAKER_POSES[] = {
getFiducial(8).getPose().toPose2d(), // BLUE
getFiducial(7).getPose().toPose2d(), // BLUE
getFiducial(4).getPose().toPose2d(), // RED
};

Expand Down
12 changes: 6 additions & 6 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,8 @@ public static RobotType fromString(String serialNum) {
double DT = 0.02;

public interface Swerve {
double WIDTH = Units.inchesToMeters(26);
double LENGTH = Units.inchesToMeters(26);
double WIDTH = Units.inchesToMeters(21);
double LENGTH = Units.inchesToMeters(21);

SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module velocity deadband (m per s)", 0.02);
double MAX_MODULE_SPEED = 5.88;
Expand All @@ -63,13 +63,13 @@ public interface Turn {
}

public interface Drive {
double kP = 0.00019162;
double kP = 0.002794;
double kI = 0.0;
double kD = 0.0;

double kS = 0.36493;
double kV = 2.448;
double kA = 0.16408;
double kS = 0.17313;
double kV = 1.7573;
double kA = 0.19554 + 0.1;
}

public interface Motion {
Expand Down
14 changes: 13 additions & 1 deletion src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ public SwerveDrive(AbstractModule... modules) {

public void configureAutoBuilder() {
AbstractOdometry odometry = AbstractOdometry.getInstance();

AutoBuilder.configureHolonomic(
odometry::getPose,
odometry::reset,
Expand Down Expand Up @@ -178,6 +178,10 @@ public void setModuleStates(SwerveModuleState... states) {
}

public void setChassisSpeeds(ChassisSpeeds speeds) {
SmartDashboard.putNumber("Swerve/X Target Chassis Speed", speeds.vxMetersPerSecond);
SmartDashboard.putNumber("Swerve/Y Target Chassis Speed", speeds.vyMetersPerSecond);
SmartDashboard.putNumber("Swerve/Omega Target Chassis Speed", speeds.omegaRadiansPerSecond);

setModuleStates(kinematics.toSwerveModuleStates(speeds));
}

Expand Down Expand Up @@ -279,6 +283,14 @@ public void periodic() {
SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getWorldLinearAccelX());
SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getWorldLinearAccelY());
SmartDashboard.putNumber("Swerve/Z Acceleration (Gs)", gyro.getWorldLinearAccelZ());

SmartDashboard.putNumber("Swerve/X Chassis Speed", getChassisSpeeds().vxMetersPerSecond);
SmartDashboard.putNumber("Swerve/Y Chassis Speed", getChassisSpeeds().vyMetersPerSecond);
SmartDashboard.putNumber("Swerve/Omega Chassis Speed", getChassisSpeeds().omegaRadiansPerSecond);

SmartDashboard.putNumber("Swerve/X Chassis Speed", getChassisSpeeds().vxMetersPerSecond);
SmartDashboard.putNumber("Swerve/Y Chassis Speed", getChassisSpeeds().vyMetersPerSecond);
SmartDashboard.putNumber("Swerve/Omega Chassis Speed", getChassisSpeeds().omegaRadiansPerSecond);
}

@Override
Expand Down

0 comments on commit fbfb2fc

Please sign in to comment.