Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

integrate RoadRunner #39

Merged
merged 1 commit into from
Sep 26, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions TeamCode/build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -29,4 +29,7 @@ dependencies {
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
implementation 'org.openftc:easyopencv:1.5.2'
implementation 'org.openftc:apriltag:1.1.0'
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,121 @@
package org.firstinspires.ftc.teamcode.drive;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;

/*
* Constants shared between multiple drive types.
*
* Constants generated by LearnRoadRunner.com/drive-constants
*
* TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
* fields may also be edited through the dashboard (connect to the robot's WiFi network and
* navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
* adjust them in the dashboard; **config variable changes don't persist between app restarts**.
*
* These are not the only parameters; some are located in the localizer classes, drive base classes,
* and op modes themselves.
*/
@Config
public class DriveConstants {
/*
* Adjust the orientations here to match your robot. See the FTC SDK documentation for details.
*/
public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR =
RevHubOrientationOnRobot.LogoFacingDirection.UP;
public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR =
RevHubOrientationOnRobot.UsbFacingDirection.LEFT;

/*
* These are motor constants that should be listed online for your motors.
*/
public static final double TICKS_PER_REV = 1120;
public static final double MAX_RPM = 150;

/*
* Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
* Set this flag to false if drive encoders are not present and an alternative localization
* method is in use (e.g., tracking wheels).
*
* If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
* from DriveVelocityPIDTuner.
*/
public static final boolean RUN_USING_ENCODER = false;
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));

/*
* These are physical constants that can be determined from your robot (including the track
* width; it will be tune empirically later although a rough estimate is important). Users are
* free to chose whichever linear distance unit they would like so long as it is consistently
* used. The default values were selected with inches in mind. Road runner uses radians for
* 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.4763; // in
public static double GEAR_RATIO = 1.06795; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 13.24; // in (originally 14.25)

/*
* These are the feedforward parameters used to model the drive motor behavior. If you are using
* the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
* motor encoders or have elected not to use them for velocity control, these values should be
* empirically tuned.
*/
// kA 0.004
// kStatic 0.02
// kV 0.045
//public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
public static double kV = 0.045;
public static double kA = 0.004;
public static double kStatic = 0.02;

/*
* These values are used to generate the trajectories for you robot. To ensure proper operation,
* the constraints should never exceed ~80% of the robot's actual capabilities. While Road
* Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
/*
* Note from LearnRoadRunner.com:
* The velocity and acceleration constraints were calculated based on the following equation:
* ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85
* Resulting in 19.7112162466021 in/s.
* This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
* This is capped at 85% because there are a number of variables that will prevent your bot from actually
* reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
* However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically
* max velocity. The theoretically maximum velocity is 23.189666172473057 in/s.
* Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
* affected if it is aiming for a velocity not actually possible.
*
* The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
* actual testing. Just set it at a reasonable value and keep increasing until your path following starts
* to degrade. As of now, it simply mirrors the velocity, resulting in 19.7112162466021 in/s/s
*
* Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
* You are free to raise this on your own if you would like. It is best determined through experimentation.

*/
public static double MAX_VEL = 19.7112162466021;
public static double MAX_ACCEL = 19.7112162466021;
//public static double MAX_ANG_VEL = Math.toRadians(79.25399999999999);
public static double MAX_ANG_VEL = Math.toRadians(82.4); // This is recommended variable; actual max is 103.0
public static double MAX_ANG_ACCEL = Math.toRadians(79.25399999999999);


public static double encoderTicksToInches(double ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
}

public static double rpmToVelocity(double rpm) {
return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
}

public static double getMotorVelocityF(double ticksPerSecond) {
// see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
return 32767 / ticksPerSecond;
}
}
Loading