Skip to content

Commit

Permalink
Merge pull request #23 from frc-4931/feature/fix-drivetrain
Browse files Browse the repository at this point in the history 
Feature/fix drivetrain
  • Loading branch information
@b-ely
b-ely authored Apr 8, 2022
2 parents ad60eaa + 243f0cb commit c71aca3
Show file tree
Hide file tree
Showing 19 changed files with 1,312 additions and 254 deletions.
2 changes: 1 addition & 1 deletion build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO

plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2022.3.1"
id "edu.wpi.first.GradleRIO" version "2022.4.1"
id "com.diffplug.spotless" version "6.2.0"
id "io.freefair.lombok" version "6.3.0"
}
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/robot/AutonomousRoute.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package frc.robot;

public enum AutonomousRoute {
DriveOut,
Blue1
// Blue_2
}
184 changes: 155 additions & 29 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,15 @@
package frc.robot;

import com.revrobotics.CANSparkMax.IdleMode;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import frc.robot.MotorConfig.PIDConfig;
import frc.robot.MotorConfig.SoftLimit;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
Expand All @@ -24,8 +28,16 @@ public static final class OIConstants {
public static final boolean USE_XBOX = true;
public static final int DRIVER_1 = 0;
public static final int DRIVER_2 = 1;
public static final int DRIVER_3 = 2;

public static final int FRONT_CAMERA = 0;
public static final String FRONT_CAMERA_NAME = "front-camera";

public static final double NOTIFY_RATE = 2; // rumble drive feedback once every <rate> seconds;
public static final double RUMBLE_LEFT_LOCKED = .5;
public static final double RUMBLE_RIGHT_LOCKED = 0d;
public static final double RUMBLE_LEFT_NO_TARGET = .9;
public static final double RUMBLE_RIGHT_NO_TARGET = .9;
}

/** Constants related to the Autonomous mode. */
Expand All @@ -46,21 +58,23 @@ public static final class AutoConstants {
}

public static final class DriveConstants {
private static final double OPEN_RAMP_RATE = 0.75;
private static final double WHEEL_DIAMETER_M = Units.inchesToMeters(8);
private static final double DRIVE_GEAR_RATIO = (14d / 70d) * (66d / 30d);
private static final double DRIVE_GEAR_RATIO = (70d / 14d) * (66d / 30d);
private static final double ENCODER_POSITION_CONVERSION =
Math.PI * WHEEL_DIAMETER_M / DRIVE_GEAR_RATIO;
// TODO: Find actual values for these!
private static final PIDConfig PID_DEFAULTS =
PIDConfig.builder()
.kP(0.8)
.kI(0)
.kP(5e-5)
.kI(1e-6)
.kD(0)
.kFF(5)
// .maxAcceleration(maxAcceleration)
// .maxVelocity(maxVelocity)
// .outputRangeHigh(outputRangeHigh)
// .outputRangeLow(outputRangeLow)
.kFF(0.000156)
.maxAcceleration(1500)
.maxVelocity(2000)
.outputRangeHigh(1)
.outputRangeLow(-1)
.allowedClosedLoopError(1)
// .minOutputVelocity(minOutputVelocity)
.build();
public static final MotorConfig FRONT_LEFT =
Expand All @@ -69,21 +83,24 @@ public static final class DriveConstants {
.idleMode(IdleMode.kBrake)
.pidConfig(PID_DEFAULTS)
.positionConversionFactor(ENCODER_POSITION_CONVERSION)
.openLoopRampRate(OPEN_RAMP_RATE)
.build();
public static final MotorConfig FRONT_RIGHT =
MotorConfig.builder()
.canId(5)
.inverted(true)
.idleMode(IdleMode.kBrake)
.pidConfig(PID_DEFAULTS)
.positionConversionFactor(ENCODER_POSITION_CONVERSION)
.openLoopRampRate(OPEN_RAMP_RATE)
.build();
public static final MotorConfig REAR_LEFT =
MotorConfig.builder()
.canId(8)
.inverted(true)
.idleMode(IdleMode.kBrake)
.pidConfig(PID_DEFAULTS)
.positionConversionFactor(ENCODER_POSITION_CONVERSION)
.openLoopRampRate(OPEN_RAMP_RATE)
.build();
public static final MotorConfig REAR_RIGHT =
MotorConfig.builder()
Expand All @@ -92,10 +109,11 @@ public static final class DriveConstants {
.inverted(true)
.pidConfig(PID_DEFAULTS)
.positionConversionFactor(ENCODER_POSITION_CONVERSION)
.openLoopRampRate(OPEN_RAMP_RATE)
.build();
}

public static final int PIGEON_MOTOR_PORT = 1;

public static final class PoseConstants {
// Distance between centers of right and left wheels on robot
public static final double kTrackWidth = Units.inchesToMeters(22.09);
// Distance between centers of front and back wheels on robot
Expand All @@ -109,41 +127,149 @@ public static final class DriveConstants {
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));

public static final int kEncoderCPR = 4096;
public static final double kWheelDiameterMeters = Units.inchesToMeters(8);
public static final double kWheelDiameterMeters = Units.inchesToMeters(6);
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;

// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These characterization values MUST be determined either experimentally or theoretically
// for *your* robot's drive.
// The SysId tool provides a convenient method for obtaining these values for your robot.
// public static final SimpleMotorFeedforward kFeedforward =
// new SimpleMotorFeedforward(1, 0.8, 0.15);

// // Example value only - as above, this must be tuned for your drive!
// public static final double kPFrontLeftVel = 0.5;
// public static final double kPRearLeftVel = 0.5;
// public static final double kPFrontRightVel = 0.5;
// public static final double kPRearRightVel = 0.5;
// Physical location of the camera on the robot, relative to the center of the
// robot.
public static final Transform2d CAMERA_TO_ROBOT =
new Transform2d(
new Translation2d(-0.25, 0), // in meters (x,y)
new Rotation2d());

// 4 ft. 5⅜ in diameter
private static final double targetWidth = Units.inchesToMeters(4 * 12 + 5 + 3 / 8); // meters

// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
// page 197
private static final double targetHeight =
Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters
public static final double targetHeightAboveGround = Units.inchesToMeters(81.19); // meters

// See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
// pages 4 and 5
public static final double kFarTgtXPos = Units.feetToMeters(54);
public static final double kFarTgtYPos =
Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
public static final Pose2d kFarTargetPose =
new Pose2d(new Translation2d(kFarTgtXPos, kFarTgtYPos), new Rotation2d(0.0));
}

public static final class IntakeConstants {
public static final MotorConfig INTAKE_MOTOR =
MotorConfig.builder()
.canId(6)
.idleMode(IdleMode.kCoast)
.pidConfig(
PIDConfig.builder()
.kP(0.74646)
.kI(0)
.kD(0)
.kFF(0.000156)
// .maxAcceleration(maxAcceleration)
// .maxVelocity(maxVelocity)
// .outputRangeHigh(1)
// .outputRangeLow(-1)
// .minOutputVelocity(minOutputVelocity)
.build())
.build();

public static final MotorConfig INTAKE_LIFT_MOTOR =
MotorConfig.builder()
.canId(12)
.idleMode(IdleMode.kBrake)
// .inverted(true)
.pidConfig(
PIDConfig.builder()
.kP(0.00005)
.kI(0.000001)
.kD(0)
.kFF(0.000156)
.maxVelocity(4700)
.maxAcceleration(8000)
.allowedClosedLoopError(0)
.build())
.pidConfig(
PIDConfig.builder()
.kP(0.00005)
.kI(0.000001)
.kD(0)
.kFF(0.000156)
.maxVelocity(5700)
.maxAcceleration(2500)
.allowedClosedLoopError(0)
.build())
.softLimitForward(SoftLimit.builder().limit(29).build())
.softLimitReverse(SoftLimit.builder().limit(-.5f).build())
.openLoopRampRate(5)
// .m
.build();
public static final double UP_POSITION = 0;
public static final double DOWN_POSITION = 29;
public static final double LIFT_THRESHOLD = .5 * DOWN_POSITION;
public static final double WAIT_BEFORE_STARTIING = 1;
public static final double SPEED = 5700 * .90;
}

public static final class ElevatorConstants {
// TODO: find these values from SimId
private static final PIDConfig PID_DEFAULTS =
PIDConfig.builder()
.kP(0.8)
.kP(0.67755)
.kI(0)
.kD(0)
.kFF(5)
.kFF(0.000156)
// .maxAcceleration(maxAcceleration)
// .maxVelocity(maxVelocity)
// .outputRangeHigh(outputRangeHigh)
// .outputRangeLow(outputRangeLow)
// .minOutputVelocity(minOutputVelocity)
.build();
public static final MotorConfig INTAKE_MOTOR =
MotorConfig.builder().canId(4).idleMode(IdleMode.kCoast).pidConfig(PID_DEFAULTS).build();
public static final int SPEED = 5700;
public static final MotorConfig ELEVATOR_MOTOR =
MotorConfig.builder().canId(7).idleMode(IdleMode.kCoast).pidConfig(PID_DEFAULTS).build();

public static final double SPEED = 5700 * .35;
}

public static final class ShooterConstants {
private static final PIDConfig PID_DEFAULTS =
PIDConfig.builder()
.kP(0.098557)
.kI(0)
.kD(0)
.kFF(0.000156)
.maxAcceleration(14000)
.maxVelocity(5700)
.outputRangeHigh(1)
.outputRangeLow(-1)
.allowedClosedLoopError(.8)
// .minOutputVelocity(minOutputVelocity)
.build();
public static final MotorConfig SHOOTER_MOTOR =
MotorConfig.builder()
.canId(13)
.idleMode(IdleMode.kCoast)
.pidConfig(PID_DEFAULTS)
// TODO: if we add a second motor
// .follower(
//
// MotorConfig.builder().canId(15).idleMode(IdleMode.kCoast).pidConfig(PID_DEFAULTS).inverted(true).build()
// )
.build();
}

public static final class SensorConstants {
public static final String CAMERA = "photoncamera"; // TODO:
public static final int PIGEON_MOTOR_PORT = 2;
}

public static final class VisionConstants {
public static final double HEIGHT_TO_GOAL = Units.inchesToMeters(8 * 12 + 5 + 3 / 8);
public static final double HEIGHT_TO_CAMERA = Units.inchesToMeters(45); // FIXME:
public static final double HEIGHT_FROM_CAMERA_TO_GOAL = HEIGHT_TO_GOAL - HEIGHT_TO_CAMERA;
public static final double CAMERA_TILT = 20; // degrees
}
}
46 changes: 42 additions & 4 deletions src/main/java/frc/robot/MotorConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.SparkMaxPIDController;
import java.util.List;
Expand All @@ -15,20 +16,40 @@ public class MotorConfig {
private int canId;
@Builder.Default private double closedLoopRampRate = 0;
@Builder.Default private double openLoopRampRate = 0;
@Builder.Default private MotorType type = MotorType.kBrushless;
private SoftLimit softLimitForward;
private SoftLimit softLimitReverse;
@Builder.Default private IdleMode idleMode = IdleMode.kBrake;
@Builder.Default private boolean inverted = false;
@Builder.Default private double positionConversionFactor = 1;
@Singular private List<PIDConfig> pidConfigs;
private MotorConfig follower;

public CANSparkMax createMotor() {
return createMotor(true);
}

private CANSparkMax createMotor(boolean burnFlash) {
var motor = new CANSparkMax(getCanId(), MotorType.kBrushless);
motor.restoreFactoryDefaults();
motor.setClosedLoopRampRate(getClosedLoopRampRate());
motor.setIdleMode(getIdleMode());
motor.setInverted(isInverted());
// motor.enableVoltageCompensation(nominalVoltage);
// motor.setSmartCurrentLimit(stallLimit, freeLimit)
motor.setOpenLoopRampRate(getOpenLoopRampRate());
if (getSoftLimitForward() != null) {
motor.enableSoftLimit(SoftLimitDirection.kForward, true);
motor.setSoftLimit(SoftLimitDirection.kForward, getSoftLimitForward().getLimit());
}
if (getSoftLimitReverse() != null) {
motor.enableSoftLimit(SoftLimitDirection.kReverse, true);
motor.setSoftLimit(SoftLimitDirection.kReverse, getSoftLimitReverse().getLimit());
}

motor.getEncoder().setPositionConversionFactor(getPositionConversionFactor());
motor.getEncoder().setVelocityConversionFactor(getPositionConversionFactor() / 60);
motor.getEncoder().setPosition(0);

SparkMaxPIDController pidController = motor.getPIDController();
for (int i = 0; i < getPidConfigs().size(); i++) {
Expand All @@ -45,7 +66,18 @@ public CANSparkMax createMotor() {
pidController.setSmartMotionMinOutputVelocity(pidConfig.getMinOutputVelocity(), i);
}

motor.burnFlash();
if (follower != null) {
boolean followerInverted = follower.inverted;
follower.inverted = this.inverted;

var followerMotor = follower.createMotor(false);
followerMotor.follow(motor, followerInverted);
followerMotor.burnFlash();
}

if (burnFlash) {
motor.burnFlash();
}
return motor;
}

Expand All @@ -56,11 +88,17 @@ public static final class PIDConfig {
private double kI;
private double kD;
private double kFF;
private double outputRangeLow;
private double outputRangeHigh;
private double allowedClosedLoopError;
@Builder.Default private double outputRangeLow = -1d;
@Builder.Default private double outputRangeHigh = 1d;
@Builder.Default private double allowedClosedLoopError = 0;
private double maxAcceleration;
private double maxVelocity;
private double minOutputVelocity;
}

@Builder
@Getter
public static final class SoftLimit {
private float limit;
}
}
Loading

0 comments on commit c71aca3

Please sign in to comment.