TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 08:32:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Road Runner Stuff, Constants are filled in but we need to do things with the Ka, and Kv variables (we run tests to find that)

Browse Source
This commit is contained in:
SpencerPiha
2023-01-28 18:32:25 -06:00
parent c262207bc6
commit 733939c0b6
5 changed files with 73 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java
View File

@@ -18,8 +18,8 @@ public class DriveConstants {
/*
* These are motor constants that should be listed online for your motors.
*/
public static final double TICKS_PER_REV = 28;
public static final double MAX_RPM = 6000;
public static final double TICKS_PER_REV = 560;
public static final double MAX_RPM = 300;
/*
* Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
@@ -30,7 +30,7 @@ public class DriveConstants {
* from DriveVelocityPIDTuner.
*/
public static final boolean RUN_USING_ENCODER = true;
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(2.5, 0.1, 0.2,
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
/*
@@ -41,9 +41,9 @@ public class DriveConstants {
* angular distances although most angular parameters are wrapped in Math.toRadians() for
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
*/
public static double WHEEL_RADIUS = 1.47637796; // in
public static double GEAR_RATIO = 20; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 8; // in
public static double WHEEL_RADIUS = 1.47637795; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 10; // in
/*
* These are the feedforward parameters used to model the drive motor behavior. If you are using
@@ -62,8 +62,8 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
public static double MAX_VEL = 12;
public static double MAX_ACCEL = 3;
public static double MAX_VEL = 37.10542498;
public static double MAX_ACCEL = 37.10542498;
public static double MAX_ANG_VEL = Math.toRadians(60);
public static double MAX_ANG_ACCEL = Math.toRadians(60);

29
TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java
View File

@@ -21,6 +21,7 @@ import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.VoltageSensor;
@@ -46,6 +47,7 @@ import org.RoadRunner.trajectorysequence.TrajectorySequence;
import org.RoadRunner.trajectorysequence.TrajectorySequenceBuilder;
import org.RoadRunner.trajectorysequence.TrajectorySequenceRunner;
import org.RoadRunner.util.LynxModuleUtil;
import org.firstinspires.ftc.robotcore.external.Telemetry;
/*
* Simple mecanum drive hardware implementation for REV hardware.
@@ -60,6 +62,7 @@ public class SampleMecanumDrive extends MecanumDrive {
public static double VX_WEIGHT = 1;
public static double VY_WEIGHT = 1;
public static double OMEGA_WEIGHT = 1;
public final StandardTrackingWheelLocalizer localizer;
private TrajectorySequenceRunner trajectorySequenceRunner;
@@ -116,10 +119,15 @@ public class SampleMecanumDrive extends MecanumDrive {
// For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
// BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y);
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
leftFront = hardwareMap.get(DcMotorEx.class, "Front Left");
leftRear = hardwareMap.get(DcMotorEx.class, "Back Left");
rightRear = hardwareMap.get(DcMotorEx.class, "Back Right");
rightFront = hardwareMap.get(DcMotorEx.class, "Front Right");
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
rightRear.setDirection(DcMotorSimple.Direction.FORWARD);
rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
@@ -140,9 +148,15 @@ public class SampleMecanumDrive extends MecanumDrive {
}
// TODO: reverse any motors using DcMotor.setDirection()
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
rightRear.setDirection(DcMotorSimple.Direction.FORWARD);
rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
// TODO: if desired, use setLocalizer() to change the localization method
// for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
localizer = new StandardTrackingWheelLocalizer(hardwareMap);
setLocalizer(localizer);
trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
}
@@ -296,6 +310,13 @@ public class SampleMecanumDrive extends MecanumDrive {
return imu.getAngularOrientation().firstAngle;
}
public void telemetry(Telemetry telemetry) {
telemetry.addData("front left", leftFront.getPower());
telemetry.addData("back left", leftRear.getPower());
telemetry.addData("front right", rightFront.getPower());
telemetry.addData("back right", rightRear.getPower());
}
@Override
public Double getExternalHeadingVelocity() {
return (double) imu.getAngularVelocity().zRotationRate;

46
TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java
View File

@@ -6,9 +6,11 @@ import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.RoadRunner.util.Encoder;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import java.util.Arrays;
import java.util.List;
@@ -28,12 +30,18 @@ import java.util.List;
*/
@Config
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
public static double TICKS_PER_REV = 0;
public static double WHEEL_RADIUS = 2; // in
public static double TICKS_PER_REV = 8192;
public static double WHEEL_RADIUS = 1.47637795; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel
public static double LATERAL_DISTANCE = 9.2913393; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = 3.54833; // in; offset of the lateral wheel
public static double X_MULTIPLIER = 0.8039; // Multiplier in the X direction
// 9.14228723
// 8.65411042
public static double Y_MULTIPLIER = 0.8039; // Multiplier in the Y direction
private Encoder leftEncoder, rightEncoder, frontEncoder;
@@ -44,11 +52,16 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer
new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
));
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder"));
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderL"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderR"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderH"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
rightEncoder.setDirection(Encoder.Direction.REVERSE);
leftEncoder.setDirection(Encoder.Direction.REVERSE);
frontEncoder.setDirection(Encoder.Direction.REVERSE);
}
public static double encoderTicksToInches(double ticks) {
@@ -59,9 +72,9 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer
@Override
public List<Double> getWheelPositions() {
return Arrays.asList(
encoderTicksToInches(leftEncoder.getCurrentPosition()),
encoderTicksToInches(rightEncoder.getCurrentPosition()),
encoderTicksToInches(frontEncoder.getCurrentPosition())
encoderTicksToInches(leftEncoder.getCurrentPosition()) * X_MULTIPLIER,
encoderTicksToInches(rightEncoder.getCurrentPosition()) * X_MULTIPLIER,
encoderTicksToInches(frontEncoder.getCurrentPosition()) * Y_MULTIPLIER
);
}
@@ -73,9 +86,16 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer
// compensation method
return Arrays.asList(
encoderTicksToInches(leftEncoder.getRawVelocity()),
encoderTicksToInches(rightEncoder.getRawVelocity()),
encoderTicksToInches(frontEncoder.getRawVelocity())
encoderTicksToInches(leftEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
encoderTicksToInches(rightEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
encoderTicksToInches(frontEncoder.getCorrectedVelocity()) * Y_MULTIPLIER
);
}
public void telemetry(Telemetry telemetry){
telemetry.addData("right", rightEncoder.getCurrentPosition());
telemetry.addData("left", leftEncoder.getCurrentPosition());
telemetry.addData("front", frontEncoder.getCurrentPosition());
}
}

5
TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java
View File

@@ -14,7 +14,7 @@ import org.RoadRunner.drive.SampleMecanumDrive;
* exercise is to ascertain whether the localizer has been configured properly (note: the pure
* encoder localizer heading may be significantly off if the track width has not been tuned).
*/
@TeleOp(group = "drive")
@TeleOp(name = "TC LocalizationTest", group = "drive")
public class LocalizationTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
@@ -39,6 +39,9 @@ public class LocalizationTest extends LinearOpMode {
telemetry.addData("x", poseEstimate.getX());
telemetry.addData("y", poseEstimate.getY());
telemetry.addData("heading", poseEstimate.getHeading());
drive.localizer.telemetry(telemetry);
telemetry.update();
}
}

3
TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java
View File

@@ -38,6 +38,9 @@ public class StraightTest extends LinearOpMode {
telemetry.addData("finalX", poseEstimate.getX());
telemetry.addData("finalY", poseEstimate.getY());
telemetry.addData("finalHeading", poseEstimate.getHeading());
drive.telemetry(telemetry);
telemetry.update();
while (!isStopRequested() && opModeIsActive()) ;