Road Runner Added
lots of stuff no one will read these Former-commit-id: 14dcd018bd1b6f74d4b35aa33d9185aca59e9213
This commit is contained in:
parent
3cad99c122
commit
7efec6d03c
|
@ -26,5 +26,10 @@
|
||||||
<option name="name" value="Google" />
|
<option name="name" value="Google" />
|
||||||
<option name="url" value="https://dl.google.com/dl/android/maven2/" />
|
<option name="url" value="https://dl.google.com/dl/android/maven2/" />
|
||||||
</remote-repository>
|
</remote-repository>
|
||||||
|
<remote-repository>
|
||||||
|
<option name="id" value="maven" />
|
||||||
|
<option name="name" value="maven" />
|
||||||
|
<option name="url" value="https://maven.brott.dev/" />
|
||||||
|
</remote-repository>
|
||||||
</component>
|
</component>
|
||||||
</project>
|
</project>
|
|
@ -19,4 +19,6 @@ dependencies {
|
||||||
implementation project(':FtcRobotController')
|
implementation project(':FtcRobotController')
|
||||||
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
|
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
|
||||||
implementation 'org.openftc:easyopencv:1.5.0'
|
implementation 'org.openftc:easyopencv:1.5.0'
|
||||||
|
implementation 'org.apache.commons:commons-math3:3.6.1'
|
||||||
|
implementation 'com.acmerobotics.roadrunner:core:0.5.4'
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,30 +1,58 @@
|
||||||
package org.firstinspires.ftc.teamcode;
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
|
||||||
@TeleOp
|
@Autonomous
|
||||||
public class EncoderValueTester extends OpMode {
|
public class EncoderValueTester extends LinearOpMode {
|
||||||
MotorBox motors;
|
MotorBox motors;
|
||||||
|
DcMotor frontRight, frontLeft, rearRight, rearLeft;
|
||||||
|
|
||||||
public void loop(){
|
|
||||||
if (gamepad1.x)
|
public void runOpMode(){
|
||||||
motors.drivePositionX(10);
|
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
|
||||||
if (gamepad1.y)
|
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
|
||||||
motors.drivePositionY(10);
|
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
|
||||||
if (gamepad1.a)
|
rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
|
||||||
motors.turn(10);
|
|
||||||
|
waitForStart();
|
||||||
|
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rearRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rearLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
|
||||||
|
frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
rearRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
rearLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
|
||||||
|
frontRight.setTargetPosition(200);
|
||||||
|
frontLeft.setTargetPosition(200);
|
||||||
|
rearRight.setTargetPosition(200);
|
||||||
|
rearLeft.setTargetPosition(200);
|
||||||
|
|
||||||
|
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
|
||||||
|
frontRight.setPower(.5);
|
||||||
|
frontLeft.setPower(.5);
|
||||||
|
rearRight.setPower(.5);
|
||||||
|
rearLeft.setPower(.5);
|
||||||
|
while (frontLeft.isBusy() || frontRight.isBusy() || rearLeft.isBusy() || rearRight.isBusy()){
|
||||||
|
sleep(1);
|
||||||
|
}
|
||||||
|
telemetry.addData("Ran", "Ran");
|
||||||
|
telemetry.update();
|
||||||
}
|
}
|
||||||
|
|
||||||
public void init(){
|
|
||||||
motors = new MotorBox(
|
|
||||||
hardwareMap.get(DcMotor.class, "frontRight"),
|
|
||||||
hardwareMap.get(DcMotor.class, "frontLeft"),
|
|
||||||
hardwareMap.get(DcMotor.class, "rearRight"),
|
|
||||||
hardwareMap.get(DcMotor.class, "rearLeft"),
|
|
||||||
true
|
|
||||||
);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -39,10 +39,10 @@ public class MotorBox {
|
||||||
rearLeftPower *= speed;
|
rearLeftPower *= speed;
|
||||||
|
|
||||||
// applies the power
|
// applies the power
|
||||||
frontRight.setPower(-1 * frontRightPower);
|
frontRight.setPower(frontRightPower);
|
||||||
frontLeft.setPower(frontLeftPower);
|
frontLeft.setPower(-1 * frontLeftPower);
|
||||||
rearRight.setPower(-1 * rearRightPower);
|
rearRight.setPower(rearRightPower);
|
||||||
rearLeft.setPower(rearLeftPower);
|
rearLeft.setPower(-1 * rearLeftPower);
|
||||||
|
|
||||||
// return for debugging
|
// return for debugging
|
||||||
return returnVal;
|
return returnVal;
|
||||||
|
@ -96,14 +96,11 @@ public class MotorBox {
|
||||||
// changes value corresponding to turn
|
// changes value corresponding to turn
|
||||||
// two states based on if the MotorControl
|
// two states based on if the MotorControl
|
||||||
private void addTurn(double turn){
|
private void addTurn(double turn){
|
||||||
if (turn > 0) {
|
frontRightPower -= turn;
|
||||||
frontRightPower *= 2*(0.5 - turn);
|
rearRightPower -= turn;
|
||||||
rearRightPower *= 2*(0.5 - turn);
|
frontLeftPower += turn;
|
||||||
}
|
rearLeftPower +=turn;
|
||||||
if (turn < 0) {
|
|
||||||
frontLeftPower *= 2*(0.5 - turn);
|
|
||||||
rearLeftPower *= 2*(0.5 - turn);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -125,6 +122,10 @@ public class MotorBox {
|
||||||
|
|
||||||
// turns on encoders
|
// turns on encoders
|
||||||
if (toPosition) {
|
if (toPosition) {
|
||||||
|
frontRight.setTargetPosition(200);
|
||||||
|
frontLeft.setTargetPosition(200);
|
||||||
|
rearRight.setTargetPosition(200);
|
||||||
|
rearLeft.setTargetPosition(200);
|
||||||
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
|
|
@ -3,11 +3,12 @@ package org.firstinspires.ftc.teamcode;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.Servo;
|
||||||
|
|
||||||
@TeleOp
|
@TeleOp
|
||||||
public class controllerOpMode extends OpMode {
|
public class controllerOpMode extends OpMode {
|
||||||
private DcMotor frontRight, frontLeft, rearRight, rearLeft;
|
private DcMotor frontRight, frontLeft, rearRight, rearLeft, spool, spinnyBoy, intakeMotor;
|
||||||
private double drive,strafe, turn;
|
private double drive,strafe, turn, linearSpeed;
|
||||||
private double speed;
|
private double speed;
|
||||||
MotorBox driveMotors;
|
MotorBox driveMotors;
|
||||||
|
|
||||||
|
@ -19,6 +20,7 @@ public class controllerOpMode extends OpMode {
|
||||||
|
|
||||||
// checks to see if the speed change button is pressed
|
// checks to see if the speed change button is pressed
|
||||||
detectSpeedChange();
|
detectSpeedChange();
|
||||||
|
detectSlideSpeedChange();
|
||||||
|
|
||||||
// sets the driver strafe and turn values
|
// sets the driver strafe and turn values
|
||||||
// drive is negated so 1 is forward
|
// drive is negated so 1 is forward
|
||||||
|
@ -28,14 +30,28 @@ public class controllerOpMode extends OpMode {
|
||||||
|
|
||||||
// prints the speed
|
// prints the speed
|
||||||
telemetry.addData("Speed", "Current Speed = " + Math.round(speed*100));
|
telemetry.addData("Speed", "Current Speed = " + Math.round(speed*100));
|
||||||
|
telemetry.addData("Linear Slide SPeed", "Current Speed = " + Math.round(linearSpeed*100));
|
||||||
|
|
||||||
// makes it drive
|
// makes it drive
|
||||||
// result is just debug values
|
// result is just debug values
|
||||||
String result = driveMotors.drivePower(drive, strafe, turn, speed);
|
String result = driveMotors.drivePower(drive, strafe, turn, speed);
|
||||||
|
spool.setPower(gamepad2.left_stick_y*linearSpeed);
|
||||||
|
if(gamepad2.b){
|
||||||
|
spinnyBoy.setPower(0.5);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
spinnyBoy.setPower(0);
|
||||||
|
}
|
||||||
|
if(gamepad2.a){
|
||||||
|
intakeMotor.setPower(0.5);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
intakeMotor.setPower(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
//debugging
|
//debugging
|
||||||
telemetry.addData("Results", result);
|
//telemetry.addData("Results", result);
|
||||||
/*
|
/*
|
||||||
telemetry.addData("Drive", "Drive = " + drive);
|
telemetry.addData("Drive", "Drive = " + drive);
|
||||||
telemetry.addData("Strafe", "Strafe = " + strafe);
|
telemetry.addData("Strafe", "Strafe = " + strafe);
|
||||||
|
@ -55,6 +71,11 @@ public class controllerOpMode extends OpMode {
|
||||||
false
|
false
|
||||||
);
|
);
|
||||||
speed = 0.25;
|
speed = 0.25;
|
||||||
|
linearSpeed = 0.25;
|
||||||
|
spool = hardwareMap.get(DcMotor.class, "spool");
|
||||||
|
spinnyBoy = hardwareMap.get(DcMotor.class, "spinnyBoy");
|
||||||
|
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
|
||||||
|
spool.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
}
|
}
|
||||||
|
|
||||||
// detects the speed change button and changes speed accordingly
|
// detects the speed change button and changes speed accordingly
|
||||||
|
@ -70,6 +91,18 @@ public class controllerOpMode extends OpMode {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
public void detectSlideSpeedChange(){
|
||||||
|
if (gamepad2.dpad_up){
|
||||||
|
if (linearSpeed <= 1) {
|
||||||
|
linearSpeed += 0.0005;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (gamepad2.dpad_down){
|
||||||
|
if (linearSpeed >= 0) {
|
||||||
|
linearSpeed -= 0.0005;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -0,0 +1,85 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Constants shared between multiple drive types.
|
||||||
|
*
|
||||||
|
* 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 {
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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 = true;
|
||||||
|
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,19.5
|
||||||
|
/*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 = 4; // in
|
||||||
|
public static double GEAR_RATIO = 5; // output (wheel) speed / input (motor) speed
|
||||||
|
public static double TRACK_WIDTH = 15.5; // in
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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.
|
||||||
|
*/
|
||||||
|
public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
|
||||||
|
public static double kA = 0;
|
||||||
|
public static double kStatic = 0;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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.
|
||||||
|
*/
|
||||||
|
public static double MAX_VEL = 600;
|
||||||
|
public static double MAX_ACCEL = 30;
|
||||||
|
public static double MAX_ANG_VEL = Math.toRadians(60);
|
||||||
|
public static double MAX_ANG_ACCEL = Math.toRadians(60);
|
||||||
|
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,319 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive;
|
||||||
|
|
||||||
|
import androidx.annotation.NonNull;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.roadrunner.control.PIDCoefficients;
|
||||||
|
import com.acmerobotics.roadrunner.drive.DriveSignal;
|
||||||
|
import com.acmerobotics.roadrunner.drive.MecanumDrive;
|
||||||
|
import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
|
||||||
|
import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
|
||||||
|
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;
|
||||||
|
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner;
|
||||||
|
import org.firstinspires.ftc.teamcode.util.LynxModuleUtil;
|
||||||
|
|
||||||
|
import java.util.ArrayList;
|
||||||
|
import java.util.Arrays;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Simple mecanum drive hardware implementation for REV hardware.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
public class SampleMecanumDrive extends MecanumDrive {
|
||||||
|
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
|
||||||
|
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
|
||||||
|
|
||||||
|
public static double LATERAL_MULTIPLIER = 1;
|
||||||
|
|
||||||
|
public static double VX_WEIGHT = 1;
|
||||||
|
public static double VY_WEIGHT = 1;
|
||||||
|
public static double OMEGA_WEIGHT = 1;
|
||||||
|
|
||||||
|
private TrajectorySequenceRunner trajectorySequenceRunner;
|
||||||
|
|
||||||
|
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
|
||||||
|
private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
|
||||||
|
|
||||||
|
private TrajectoryFollower follower;
|
||||||
|
|
||||||
|
private DcMotorEx leftFront, leftRear, rightRear, rightFront;
|
||||||
|
private List<DcMotorEx> motors;
|
||||||
|
|
||||||
|
private BNO055IMU imu;
|
||||||
|
private VoltageSensor batteryVoltageSensor;
|
||||||
|
|
||||||
|
public SampleMecanumDrive(HardwareMap hardwareMap) {
|
||||||
|
super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
|
||||||
|
|
||||||
|
follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
|
||||||
|
new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
|
||||||
|
|
||||||
|
LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
|
||||||
|
|
||||||
|
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
|
||||||
|
|
||||||
|
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
|
||||||
|
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO: adjust the names of the following hardware devices to match your configuration
|
||||||
|
imu = hardwareMap.get(BNO055IMU.class, "imu");
|
||||||
|
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
||||||
|
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
|
||||||
|
imu.initialize(parameters);
|
||||||
|
|
||||||
|
// TODO: if your hub is mounted vertically, remap the IMU axes so that the z-axis points
|
||||||
|
// upward (normal to the floor) using a command like the following:
|
||||||
|
// BNO055IMUUtil.remapAxes(imu, AxesOrder.XYZ, AxesSigns.NPN);
|
||||||
|
|
||||||
|
leftFront = hardwareMap.get(DcMotorEx.class, "frontLeft");
|
||||||
|
leftRear = hardwareMap.get(DcMotorEx.class, "rearLeft");
|
||||||
|
rightRear = hardwareMap.get(DcMotorEx.class, "rearRight");
|
||||||
|
rightFront = hardwareMap.get(DcMotorEx.class, "frontRight");
|
||||||
|
|
||||||
|
motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
|
||||||
|
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
|
||||||
|
motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
|
||||||
|
motor.setMotorType(motorConfigurationType);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (RUN_USING_ENCODER) {
|
||||||
|
setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
}
|
||||||
|
|
||||||
|
setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
|
if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
|
||||||
|
setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO: reverse any motors using DcMotor.setDirection()
|
||||||
|
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||||
|
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||||
|
rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||||
|
rightRear.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||||
|
|
||||||
|
// TODO: if desired, use setLocalizer() to change the localization method
|
||||||
|
// for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
|
||||||
|
|
||||||
|
trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
|
||||||
|
return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
|
||||||
|
return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
|
||||||
|
return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
|
||||||
|
return new TrajectorySequenceBuilder(
|
||||||
|
startPose,
|
||||||
|
VEL_CONSTRAINT, ACCEL_CONSTRAINT,
|
||||||
|
MAX_ANG_VEL, MAX_ANG_ACCEL
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void turnAsync(double angle) {
|
||||||
|
trajectorySequenceRunner.followTrajectorySequenceAsync(
|
||||||
|
trajectorySequenceBuilder(getPoseEstimate())
|
||||||
|
.turn(angle)
|
||||||
|
.build()
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void turn(double angle) {
|
||||||
|
turnAsync(angle);
|
||||||
|
waitForIdle();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void followTrajectoryAsync(Trajectory trajectory) {
|
||||||
|
trajectorySequenceRunner.followTrajectorySequenceAsync(
|
||||||
|
trajectorySequenceBuilder(trajectory.start())
|
||||||
|
.addTrajectory(trajectory)
|
||||||
|
.build()
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void followTrajectory(Trajectory trajectory) {
|
||||||
|
followTrajectoryAsync(trajectory);
|
||||||
|
waitForIdle();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
|
||||||
|
trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
|
||||||
|
followTrajectorySequenceAsync(trajectorySequence);
|
||||||
|
waitForIdle();
|
||||||
|
}
|
||||||
|
|
||||||
|
public Pose2d getLastError() {
|
||||||
|
return trajectorySequenceRunner.getLastPoseError();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void update() {
|
||||||
|
updatePoseEstimate();
|
||||||
|
DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
|
||||||
|
if (signal != null) setDriveSignal(signal);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void waitForIdle() {
|
||||||
|
while (!Thread.currentThread().isInterrupted() && isBusy())
|
||||||
|
update();
|
||||||
|
}
|
||||||
|
|
||||||
|
public boolean isBusy() {
|
||||||
|
return trajectorySequenceRunner.isBusy();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setMode(DcMotor.RunMode runMode) {
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
motor.setMode(runMode);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
motor.setZeroPowerBehavior(zeroPowerBehavior);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
|
||||||
|
PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
|
||||||
|
coefficients.p, coefficients.i, coefficients.d,
|
||||||
|
coefficients.f * 12 / batteryVoltageSensor.getVoltage()
|
||||||
|
);
|
||||||
|
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
motor.setPIDFCoefficients(runMode, compensatedCoefficients);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setWeightedDrivePower(Pose2d drivePower) {
|
||||||
|
Pose2d vel = drivePower;
|
||||||
|
|
||||||
|
if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
|
||||||
|
+ Math.abs(drivePower.getHeading()) > 1) {
|
||||||
|
// re-normalize the powers according to the weights
|
||||||
|
double denom = VX_WEIGHT * Math.abs(drivePower.getX())
|
||||||
|
+ VY_WEIGHT * Math.abs(drivePower.getY())
|
||||||
|
+ OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
|
||||||
|
|
||||||
|
vel = new Pose2d(
|
||||||
|
VX_WEIGHT * drivePower.getX(),
|
||||||
|
VY_WEIGHT * drivePower.getY(),
|
||||||
|
OMEGA_WEIGHT * drivePower.getHeading()
|
||||||
|
).div(denom);
|
||||||
|
}
|
||||||
|
|
||||||
|
setDrivePower(vel);
|
||||||
|
}
|
||||||
|
|
||||||
|
@NonNull
|
||||||
|
@Override
|
||||||
|
public List<Double> getWheelPositions() {
|
||||||
|
List<Double> wheelPositions = new ArrayList<>();
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
wheelPositions.add(encoderTicksToInches(motor.getCurrentPosition()));
|
||||||
|
}
|
||||||
|
return wheelPositions;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public List<Double> getWheelVelocities() {
|
||||||
|
List<Double> wheelVelocities = new ArrayList<>();
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
wheelVelocities.add(encoderTicksToInches(motor.getVelocity()));
|
||||||
|
}
|
||||||
|
return wheelVelocities;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void setMotorPowers(double v, double v1, double v2, double v3) {
|
||||||
|
leftFront.setPower(v);
|
||||||
|
leftRear.setPower(v1);
|
||||||
|
rightRear.setPower(v2);
|
||||||
|
rightFront.setPower(v3);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public double getRawExternalHeading() {
|
||||||
|
return imu.getAngularOrientation().firstAngle;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public Double getExternalHeadingVelocity() {
|
||||||
|
// TODO: This must be changed to match your configuration
|
||||||
|
// | Z axis
|
||||||
|
// |
|
||||||
|
// (Motor Port Side) | / X axis
|
||||||
|
// ____|__/____
|
||||||
|
// Y axis / * | / /| (IO Side)
|
||||||
|
// _________ /______|/ // I2C
|
||||||
|
// /___________ // Digital
|
||||||
|
// |____________|/ Analog
|
||||||
|
//
|
||||||
|
// (Servo Port Side)
|
||||||
|
//
|
||||||
|
// The positive x axis points toward the USB port(s)
|
||||||
|
//
|
||||||
|
// Adjust the axis rotation rate as necessary
|
||||||
|
// Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying
|
||||||
|
// flat on a surface
|
||||||
|
|
||||||
|
return (double) imu.getAngularVelocity().zRotationRate;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
|
||||||
|
return new MinVelocityConstraint(Arrays.asList(
|
||||||
|
new AngularVelocityConstraint(maxAngularVel),
|
||||||
|
new MecanumVelocityConstraint(maxVel, trackWidth)
|
||||||
|
));
|
||||||
|
}
|
||||||
|
|
||||||
|
public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
|
||||||
|
return new ProfileAccelerationConstraint(maxAccel);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,318 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive;
|
||||||
|
|
||||||
|
import androidx.annotation.NonNull;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.roadrunner.control.PIDCoefficients;
|
||||||
|
import com.acmerobotics.roadrunner.drive.DriveSignal;
|
||||||
|
import com.acmerobotics.roadrunner.drive.TankDrive;
|
||||||
|
import com.acmerobotics.roadrunner.followers.TankPIDVAFollower;
|
||||||
|
import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
|
||||||
|
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.HardwareMap;
|
||||||
|
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
||||||
|
import com.qualcomm.robotcore.hardware.VoltageSensor;
|
||||||
|
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner;
|
||||||
|
import org.firstinspires.ftc.teamcode.util.LynxModuleUtil;
|
||||||
|
|
||||||
|
import java.util.Arrays;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Simple tank drive hardware implementation for REV hardware.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
public class SampleTankDrive extends TankDrive {
|
||||||
|
public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0);
|
||||||
|
public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0);
|
||||||
|
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
|
||||||
|
|
||||||
|
public static double VX_WEIGHT = 1;
|
||||||
|
public static double OMEGA_WEIGHT = 1;
|
||||||
|
|
||||||
|
private TrajectorySequenceRunner trajectorySequenceRunner;
|
||||||
|
|
||||||
|
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
|
||||||
|
private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL);
|
||||||
|
|
||||||
|
private TrajectoryFollower follower;
|
||||||
|
|
||||||
|
private List<DcMotorEx> motors, leftMotors, rightMotors;
|
||||||
|
private BNO055IMU imu;
|
||||||
|
|
||||||
|
private VoltageSensor batteryVoltageSensor;
|
||||||
|
|
||||||
|
public SampleTankDrive(HardwareMap hardwareMap) {
|
||||||
|
super(kV, kA, kStatic, TRACK_WIDTH);
|
||||||
|
|
||||||
|
follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID,
|
||||||
|
new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
|
||||||
|
|
||||||
|
LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
|
||||||
|
|
||||||
|
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
|
||||||
|
|
||||||
|
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
|
||||||
|
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO: adjust the names of the following hardware devices to match your configuration
|
||||||
|
imu = hardwareMap.get(BNO055IMU.class, "imu");
|
||||||
|
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
||||||
|
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
|
||||||
|
imu.initialize(parameters);
|
||||||
|
|
||||||
|
// TODO: if your hub is mounted vertically, remap the IMU axes so that the z-axis points
|
||||||
|
// upward (normal to the floor) using a command like the following:
|
||||||
|
// BNO055IMUUtil.remapAxes(imu, AxesOrder.XYZ, AxesSigns.NPN);
|
||||||
|
|
||||||
|
// add/remove motors depending on your robot (e.g., 6WD)
|
||||||
|
DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
|
||||||
|
DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
|
||||||
|
DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
|
||||||
|
DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
|
||||||
|
|
||||||
|
motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
|
||||||
|
leftMotors = Arrays.asList(leftFront, leftRear);
|
||||||
|
rightMotors = Arrays.asList(rightFront, rightRear);
|
||||||
|
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
|
||||||
|
motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
|
||||||
|
motor.setMotorType(motorConfigurationType);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (RUN_USING_ENCODER) {
|
||||||
|
setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
}
|
||||||
|
|
||||||
|
setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
|
if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
|
||||||
|
setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO: reverse any motors using DcMotor.setDirection()
|
||||||
|
|
||||||
|
// TODO: if desired, use setLocalizer() to change the localization method
|
||||||
|
// for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
|
||||||
|
|
||||||
|
trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
|
||||||
|
return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
|
||||||
|
return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
|
||||||
|
return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
|
||||||
|
return new TrajectorySequenceBuilder(
|
||||||
|
startPose,
|
||||||
|
VEL_CONSTRAINT, accelConstraint,
|
||||||
|
MAX_ANG_VEL, MAX_ANG_ACCEL
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void turnAsync(double angle) {
|
||||||
|
trajectorySequenceRunner.followTrajectorySequenceAsync(
|
||||||
|
trajectorySequenceBuilder(getPoseEstimate())
|
||||||
|
.turn(angle)
|
||||||
|
.build()
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void turn(double angle) {
|
||||||
|
turnAsync(angle);
|
||||||
|
waitForIdle();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void followTrajectoryAsync(Trajectory trajectory) {
|
||||||
|
trajectorySequenceRunner.followTrajectorySequenceAsync(
|
||||||
|
trajectorySequenceBuilder(trajectory.start())
|
||||||
|
.addTrajectory(trajectory)
|
||||||
|
.build()
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void followTrajectory(Trajectory trajectory) {
|
||||||
|
followTrajectoryAsync(trajectory);
|
||||||
|
waitForIdle();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
|
||||||
|
trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
|
||||||
|
followTrajectorySequenceAsync(trajectorySequence);
|
||||||
|
waitForIdle();
|
||||||
|
}
|
||||||
|
|
||||||
|
public Pose2d getLastError() {
|
||||||
|
return trajectorySequenceRunner.getLastPoseError();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public void update() {
|
||||||
|
updatePoseEstimate();
|
||||||
|
DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
|
||||||
|
if (signal != null) setDriveSignal(signal);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void waitForIdle() {
|
||||||
|
while (!Thread.currentThread().isInterrupted() && isBusy())
|
||||||
|
update();
|
||||||
|
}
|
||||||
|
|
||||||
|
public boolean isBusy() {
|
||||||
|
return trajectorySequenceRunner.isBusy();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setMode(DcMotor.RunMode runMode) {
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
motor.setMode(runMode);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
motor.setZeroPowerBehavior(zeroPowerBehavior);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
|
||||||
|
PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
|
||||||
|
coefficients.p, coefficients.i, coefficients.d,
|
||||||
|
coefficients.f * 12 / batteryVoltageSensor.getVoltage()
|
||||||
|
);
|
||||||
|
for (DcMotorEx motor : motors) {
|
||||||
|
motor.setPIDFCoefficients(runMode, compensatedCoefficients);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setWeightedDrivePower(Pose2d drivePower) {
|
||||||
|
Pose2d vel = drivePower;
|
||||||
|
|
||||||
|
if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) {
|
||||||
|
// re-normalize the powers according to the weights
|
||||||
|
double denom = VX_WEIGHT * Math.abs(drivePower.getX())
|
||||||
|
+ OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
|
||||||
|
|
||||||
|
vel = new Pose2d(
|
||||||
|
VX_WEIGHT * drivePower.getX(),
|
||||||
|
0,
|
||||||
|
OMEGA_WEIGHT * drivePower.getHeading()
|
||||||
|
).div(denom);
|
||||||
|
}
|
||||||
|
|
||||||
|
setDrivePower(vel);
|
||||||
|
}
|
||||||
|
|
||||||
|
@NonNull
|
||||||
|
@Override
|
||||||
|
public List<Double> getWheelPositions() {
|
||||||
|
double leftSum = 0, rightSum = 0;
|
||||||
|
for (DcMotorEx leftMotor : leftMotors) {
|
||||||
|
leftSum += encoderTicksToInches(leftMotor.getCurrentPosition());
|
||||||
|
}
|
||||||
|
for (DcMotorEx rightMotor : rightMotors) {
|
||||||
|
rightSum += encoderTicksToInches(rightMotor.getCurrentPosition());
|
||||||
|
}
|
||||||
|
return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
|
||||||
|
}
|
||||||
|
|
||||||
|
public List<Double> getWheelVelocities() {
|
||||||
|
double leftSum = 0, rightSum = 0;
|
||||||
|
for (DcMotorEx leftMotor : leftMotors) {
|
||||||
|
leftSum += encoderTicksToInches(leftMotor.getVelocity());
|
||||||
|
}
|
||||||
|
for (DcMotorEx rightMotor : rightMotors) {
|
||||||
|
rightSum += encoderTicksToInches(rightMotor.getVelocity());
|
||||||
|
}
|
||||||
|
return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void setMotorPowers(double v, double v1) {
|
||||||
|
for (DcMotorEx leftMotor : leftMotors) {
|
||||||
|
leftMotor.setPower(v);
|
||||||
|
}
|
||||||
|
for (DcMotorEx rightMotor : rightMotors) {
|
||||||
|
rightMotor.setPower(v1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public double getRawExternalHeading() {
|
||||||
|
return imu.getAngularOrientation().firstAngle;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public Double getExternalHeadingVelocity() {
|
||||||
|
// TODO: This must be changed to match your configuration
|
||||||
|
// | Z axis
|
||||||
|
// |
|
||||||
|
// (Motor Port Side) | / X axis
|
||||||
|
// ____|__/____
|
||||||
|
// Y axis / * | / /| (IO Side)
|
||||||
|
// _________ /______|/ // I2C
|
||||||
|
// /___________ // Digital
|
||||||
|
// |____________|/ Analog
|
||||||
|
//
|
||||||
|
// (Servo Port Side)
|
||||||
|
//
|
||||||
|
// The positive x axis points toward the USB port(s)
|
||||||
|
//
|
||||||
|
// Adjust the axis rotation rate as necessary
|
||||||
|
// Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying
|
||||||
|
// flat on a surface
|
||||||
|
|
||||||
|
return (double) imu.getAngularVelocity().zRotationRate;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
|
||||||
|
return new MinVelocityConstraint(Arrays.asList(
|
||||||
|
new AngularVelocityConstraint(maxAngularVel),
|
||||||
|
new TankVelocityConstraint(maxVel, trackWidth)
|
||||||
|
));
|
||||||
|
}
|
||||||
|
|
||||||
|
public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
|
||||||
|
return new ProfileAccelerationConstraint(maxAccel);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,80 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive;
|
||||||
|
|
||||||
|
import androidx.annotation.NonNull;
|
||||||
|
|
||||||
|
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.HardwareMap;
|
||||||
|
import org.firstinspires.ftc.teamcode.util.Encoder;
|
||||||
|
|
||||||
|
import java.util.Arrays;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Sample tracking wheel localizer implementation assuming the standard configuration:
|
||||||
|
*
|
||||||
|
* /--------------\
|
||||||
|
* | ____ |
|
||||||
|
* | ---- |
|
||||||
|
* | || || |
|
||||||
|
* | || || |
|
||||||
|
* | |
|
||||||
|
* | |
|
||||||
|
* \--------------/
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
|
||||||
|
public static double TICKS_PER_REV = 0;
|
||||||
|
public static double WHEEL_RADIUS = 2; // 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
|
||||||
|
|
||||||
|
private Encoder leftEncoder, rightEncoder, frontEncoder;
|
||||||
|
|
||||||
|
public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
|
||||||
|
super(Arrays.asList(
|
||||||
|
new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
|
||||||
|
new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
|
||||||
|
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"));
|
||||||
|
|
||||||
|
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
|
||||||
|
}
|
||||||
|
|
||||||
|
public static double encoderTicksToInches(double ticks) {
|
||||||
|
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
|
||||||
|
}
|
||||||
|
|
||||||
|
@NonNull
|
||||||
|
@Override
|
||||||
|
public List<Double> getWheelPositions() {
|
||||||
|
return Arrays.asList(
|
||||||
|
encoderTicksToInches(leftEncoder.getCurrentPosition()),
|
||||||
|
encoderTicksToInches(rightEncoder.getCurrentPosition()),
|
||||||
|
encoderTicksToInches(frontEncoder.getCurrentPosition())
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
@NonNull
|
||||||
|
@Override
|
||||||
|
public List<Double> getWheelVelocities() {
|
||||||
|
// TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
|
||||||
|
// competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
|
||||||
|
// compensation method
|
||||||
|
|
||||||
|
return Arrays.asList(
|
||||||
|
encoderTicksToInches(leftEncoder.getRawVelocity()),
|
||||||
|
encoderTicksToInches(rightEncoder.getRawVelocity()),
|
||||||
|
encoderTicksToInches(frontEncoder.getRawVelocity())
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,220 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.util.NanoClock;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.util.RobotLog;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.internal.system.Misc;
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
import org.firstinspires.ftc.teamcode.util.LoggingUtil;
|
||||||
|
import org.firstinspires.ftc.teamcode.util.RegressionUtil;
|
||||||
|
|
||||||
|
import java.util.ArrayList;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_RPM;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.rpmToVelocity;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
|
||||||
|
* outline of the procedure:
|
||||||
|
* 1. Slowly ramp the motor power and record encoder values along the way.
|
||||||
|
* 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
|
||||||
|
* and an optional intercept (kStatic).
|
||||||
|
* 3. Accelerate the robot (apply constant power) and record the encoder counts.
|
||||||
|
* 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
|
||||||
|
* regression.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class AutomaticFeedforwardTuner extends LinearOpMode {
|
||||||
|
public static double MAX_POWER = 0.7;
|
||||||
|
public static double DISTANCE = 100; // in
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
if (RUN_USING_ENCODER) {
|
||||||
|
RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
|
||||||
|
"when using the built-in drive motor velocity PID.");
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
NanoClock clock = NanoClock.system();
|
||||||
|
|
||||||
|
telemetry.addLine("Press play to begin the feedforward tuning routine");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Would you like to fit kStatic?");
|
||||||
|
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
boolean fitIntercept = false;
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
if (gamepad1.y) {
|
||||||
|
fitIntercept = true;
|
||||||
|
while (!isStopRequested() && gamepad1.y) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
} else if (gamepad1.b) {
|
||||||
|
while (!isStopRequested() && gamepad1.b) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine(Misc.formatInvariant(
|
||||||
|
"Place your robot on the field with at least %.2f in of room in front", DISTANCE));
|
||||||
|
telemetry.addLine("Press (Y/Δ) to begin");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
while (!isStopRequested() && !gamepad1.y) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
while (!isStopRequested() && gamepad1.y) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Running...");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
double maxVel = rpmToVelocity(MAX_RPM);
|
||||||
|
double finalVel = MAX_POWER * maxVel;
|
||||||
|
double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
|
||||||
|
double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
|
||||||
|
|
||||||
|
List<Double> timeSamples = new ArrayList<>();
|
||||||
|
List<Double> positionSamples = new ArrayList<>();
|
||||||
|
List<Double> powerSamples = new ArrayList<>();
|
||||||
|
|
||||||
|
drive.setPoseEstimate(new Pose2d());
|
||||||
|
|
||||||
|
double startTime = clock.seconds();
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
double elapsedTime = clock.seconds() - startTime;
|
||||||
|
if (elapsedTime > rampTime) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
double vel = accel * elapsedTime;
|
||||||
|
double power = vel / maxVel;
|
||||||
|
|
||||||
|
timeSamples.add(elapsedTime);
|
||||||
|
positionSamples.add(drive.getPoseEstimate().getX());
|
||||||
|
powerSamples.add(power);
|
||||||
|
|
||||||
|
drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
}
|
||||||
|
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
|
||||||
|
|
||||||
|
RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData(
|
||||||
|
timeSamples, positionSamples, powerSamples, fitIntercept,
|
||||||
|
LoggingUtil.getLogFile(Misc.formatInvariant(
|
||||||
|
"DriveRampRegression-%d.csv", System.currentTimeMillis())));
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Quasi-static ramp up test complete");
|
||||||
|
if (fitIntercept) {
|
||||||
|
telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)",
|
||||||
|
rampResult.kV, rampResult.kStatic, rampResult.rSquare));
|
||||||
|
} else {
|
||||||
|
telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)",
|
||||||
|
rampResult.kStatic, rampResult.rSquare));
|
||||||
|
}
|
||||||
|
telemetry.addLine("Would you like to fit kA?");
|
||||||
|
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
boolean fitAccelFF = false;
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
if (gamepad1.y) {
|
||||||
|
fitAccelFF = true;
|
||||||
|
while (!isStopRequested() && gamepad1.y) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
} else if (gamepad1.b) {
|
||||||
|
while (!isStopRequested() && gamepad1.b) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (fitAccelFF) {
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Place the robot back in its starting position");
|
||||||
|
telemetry.addLine("Press (Y/Δ) to continue");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
while (!isStopRequested() && !gamepad1.y) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
while (!isStopRequested() && gamepad1.y) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Running...");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
double maxPowerTime = DISTANCE / maxVel;
|
||||||
|
|
||||||
|
timeSamples.clear();
|
||||||
|
positionSamples.clear();
|
||||||
|
powerSamples.clear();
|
||||||
|
|
||||||
|
drive.setPoseEstimate(new Pose2d());
|
||||||
|
drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
|
||||||
|
|
||||||
|
startTime = clock.seconds();
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
double elapsedTime = clock.seconds() - startTime;
|
||||||
|
if (elapsedTime > maxPowerTime) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
timeSamples.add(elapsedTime);
|
||||||
|
positionSamples.add(drive.getPoseEstimate().getX());
|
||||||
|
powerSamples.add(MAX_POWER);
|
||||||
|
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
}
|
||||||
|
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
|
||||||
|
|
||||||
|
RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData(
|
||||||
|
timeSamples, positionSamples, powerSamples, rampResult,
|
||||||
|
LoggingUtil.getLogFile(Misc.formatInvariant(
|
||||||
|
"DriveAccelRegression-%d.csv", System.currentTimeMillis())));
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Constant power test complete");
|
||||||
|
telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
|
||||||
|
accelResult.kA, accelResult.rSquare));
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,52 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
|
||||||
|
* classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
|
||||||
|
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
|
||||||
|
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
|
||||||
|
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
|
||||||
|
* you've successfully connected, start the program, and your robot will begin moving forward and
|
||||||
|
* backward. You should observe the target position (green) and your pose estimate (blue) and adjust
|
||||||
|
* your follower PID coefficients such that you follow the target position as accurately as possible.
|
||||||
|
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
|
||||||
|
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
|
||||||
|
* These coefficients can be tuned live in dashboard.
|
||||||
|
*
|
||||||
|
* This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
|
||||||
|
* is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class BackAndForth extends LinearOpMode {
|
||||||
|
|
||||||
|
public static double DISTANCE = 50;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
|
||||||
|
.forward(DISTANCE)
|
||||||
|
.build();
|
||||||
|
|
||||||
|
Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
|
||||||
|
.back(DISTANCE)
|
||||||
|
.build();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
while (opModeIsActive() && !isStopRequested()) {
|
||||||
|
drive.followTrajectory(trajectoryForward);
|
||||||
|
drive.followTrajectory(trajectoryBackward);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,170 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionProfile;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionState;
|
||||||
|
import com.acmerobotics.roadrunner.util.NanoClock;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.util.RobotLog;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
|
||||||
|
* loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
|
||||||
|
* important as the positional parameters. Like the other manual tuning routines, this op mode
|
||||||
|
* relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
|
||||||
|
* WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
|
||||||
|
* phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
|
||||||
|
* connected, start the program, and your robot will begin moving forward and backward according to
|
||||||
|
* a motion profile. Your job is to graph the velocity errors over time and adjust the PID
|
||||||
|
* coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
|
||||||
|
* Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
|
||||||
|
* MOTOR_VELO_PID field.
|
||||||
|
*
|
||||||
|
* Recommended tuning process:
|
||||||
|
*
|
||||||
|
* 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
|
||||||
|
* mitigate oscillations.
|
||||||
|
* 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
|
||||||
|
* 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
|
||||||
|
*
|
||||||
|
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
|
||||||
|
* user to reset the position of the bot in the event that it drifts off the path.
|
||||||
|
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class DriveVelocityPIDTuner extends LinearOpMode {
|
||||||
|
public static double DISTANCE = 72; // in
|
||||||
|
|
||||||
|
enum Mode {
|
||||||
|
DRIVER_MODE,
|
||||||
|
TUNING_MODE
|
||||||
|
}
|
||||||
|
|
||||||
|
private static MotionProfile generateProfile(boolean movingForward) {
|
||||||
|
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
|
||||||
|
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
|
||||||
|
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
if (!RUN_USING_ENCODER) {
|
||||||
|
RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
|
||||||
|
"PID is not in use", getClass().getSimpleName());
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
Mode mode = Mode.TUNING_MODE;
|
||||||
|
|
||||||
|
double lastKp = MOTOR_VELO_PID.p;
|
||||||
|
double lastKi = MOTOR_VELO_PID.i;
|
||||||
|
double lastKd = MOTOR_VELO_PID.d;
|
||||||
|
double lastKf = MOTOR_VELO_PID.f;
|
||||||
|
|
||||||
|
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
|
||||||
|
|
||||||
|
NanoClock clock = NanoClock.system();
|
||||||
|
|
||||||
|
telemetry.addLine("Ready!");
|
||||||
|
telemetry.update();
|
||||||
|
telemetry.clearAll();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
boolean movingForwards = true;
|
||||||
|
MotionProfile activeProfile = generateProfile(true);
|
||||||
|
double profileStart = clock.seconds();
|
||||||
|
|
||||||
|
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
telemetry.addData("mode", mode);
|
||||||
|
|
||||||
|
switch (mode) {
|
||||||
|
case TUNING_MODE:
|
||||||
|
if (gamepad1.y) {
|
||||||
|
mode = Mode.DRIVER_MODE;
|
||||||
|
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
}
|
||||||
|
|
||||||
|
// calculate and set the motor power
|
||||||
|
double profileTime = clock.seconds() - profileStart;
|
||||||
|
|
||||||
|
if (profileTime > activeProfile.duration()) {
|
||||||
|
// generate a new profile
|
||||||
|
movingForwards = !movingForwards;
|
||||||
|
activeProfile = generateProfile(movingForwards);
|
||||||
|
profileStart = clock.seconds();
|
||||||
|
}
|
||||||
|
|
||||||
|
MotionState motionState = activeProfile.get(profileTime);
|
||||||
|
double targetPower = kV * motionState.getV();
|
||||||
|
drive.setDrivePower(new Pose2d(targetPower, 0, 0));
|
||||||
|
|
||||||
|
List<Double> velocities = drive.getWheelVelocities();
|
||||||
|
|
||||||
|
// update telemetry
|
||||||
|
telemetry.addData("targetVelocity", motionState.getV());
|
||||||
|
for (int i = 0; i < velocities.size(); i++) {
|
||||||
|
telemetry.addData("measuredVelocity" + i, velocities.get(i));
|
||||||
|
telemetry.addData(
|
||||||
|
"error" + i,
|
||||||
|
motionState.getV() - velocities.get(i)
|
||||||
|
);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case DRIVER_MODE:
|
||||||
|
if (gamepad1.b) {
|
||||||
|
drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
|
||||||
|
mode = Mode.TUNING_MODE;
|
||||||
|
movingForwards = true;
|
||||||
|
activeProfile = generateProfile(movingForwards);
|
||||||
|
profileStart = clock.seconds();
|
||||||
|
}
|
||||||
|
|
||||||
|
drive.setWeightedDrivePower(
|
||||||
|
new Pose2d(
|
||||||
|
-gamepad1.left_stick_y,
|
||||||
|
-gamepad1.left_stick_x,
|
||||||
|
-gamepad1.right_stick_x
|
||||||
|
)
|
||||||
|
);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
|
||||||
|
|| lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
|
||||||
|
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
|
||||||
|
|
||||||
|
lastKp = MOTOR_VELO_PID.p;
|
||||||
|
lastKi = MOTOR_VELO_PID.i;
|
||||||
|
lastKd = MOTOR_VELO_PID.d;
|
||||||
|
lastKf = MOTOR_VELO_PID.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,55 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
|
||||||
|
* classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
|
||||||
|
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
|
||||||
|
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
|
||||||
|
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
|
||||||
|
* you've successfully connected, start the program, and your robot will begin driving in a square.
|
||||||
|
* You should observe the target position (green) and your pose estimate (blue) and adjust your
|
||||||
|
* follower PID coefficients such that you follow the target position as accurately as possible.
|
||||||
|
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
|
||||||
|
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
|
||||||
|
* These coefficients can be tuned live in dashboard.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class FollowerPIDTuner extends LinearOpMode {
|
||||||
|
public static double DISTANCE = 48; // in
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
|
||||||
|
|
||||||
|
drive.setPoseEstimate(startPose);
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
|
||||||
|
.forward(DISTANCE)
|
||||||
|
.turn(Math.toRadians(90))
|
||||||
|
.forward(DISTANCE)
|
||||||
|
.turn(Math.toRadians(90))
|
||||||
|
.forward(DISTANCE)
|
||||||
|
.turn(Math.toRadians(90))
|
||||||
|
.forward(DISTANCE)
|
||||||
|
.turn(Math.toRadians(90))
|
||||||
|
.build();
|
||||||
|
drive.followTrajectorySequence(trajSeq);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,45 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This is a simple teleop routine for testing localization. Drive the robot around like a normal
|
||||||
|
* teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
|
||||||
|
* errors are not out of the ordinary, especially with sudden drive motions). The goal of this
|
||||||
|
* 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")
|
||||||
|
public class LocalizationTest extends LinearOpMode {
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
drive.setWeightedDrivePower(
|
||||||
|
new Pose2d(
|
||||||
|
-gamepad1.left_stick_y,
|
||||||
|
-gamepad1.left_stick_x,
|
||||||
|
-gamepad1.right_stick_x
|
||||||
|
)
|
||||||
|
);
|
||||||
|
|
||||||
|
drive.update();
|
||||||
|
|
||||||
|
Pose2d poseEstimate = drive.getPoseEstimate();
|
||||||
|
telemetry.addData("x", poseEstimate.getX());
|
||||||
|
telemetry.addData("y", poseEstimate.getY());
|
||||||
|
telemetry.addData("heading", poseEstimate.getHeading());
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,146 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.kinematics.Kinematics;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionProfile;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionState;
|
||||||
|
import com.acmerobotics.roadrunner.util.NanoClock;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.util.RobotLog;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
import java.util.Objects;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
|
||||||
|
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
|
||||||
|
* tuning these coefficients is just as important as the positional parameters. Like the other
|
||||||
|
* manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
|
||||||
|
* connect your computer to the RC's WiFi network. In your browser, navigate to
|
||||||
|
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
|
||||||
|
* you are using the Control Hub. Once you've successfully connected, start the program, and your
|
||||||
|
* robot will begin moving forward and backward according to a motion profile. Your job is to graph
|
||||||
|
* the velocity errors over time and adjust the feedforward coefficients. Once you've found a
|
||||||
|
* satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
|
||||||
|
*
|
||||||
|
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
|
||||||
|
* user to reset the position of the bot in the event that it drifts off the path.
|
||||||
|
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class ManualFeedforwardTuner extends LinearOpMode {
|
||||||
|
public static double DISTANCE = 72; // in
|
||||||
|
|
||||||
|
private FtcDashboard dashboard = FtcDashboard.getInstance();
|
||||||
|
|
||||||
|
private SampleMecanumDrive drive;
|
||||||
|
|
||||||
|
enum Mode {
|
||||||
|
DRIVER_MODE,
|
||||||
|
TUNING_MODE
|
||||||
|
}
|
||||||
|
|
||||||
|
private Mode mode;
|
||||||
|
|
||||||
|
private static MotionProfile generateProfile(boolean movingForward) {
|
||||||
|
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
|
||||||
|
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
|
||||||
|
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
if (RUN_USING_ENCODER) {
|
||||||
|
RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
|
||||||
|
"when using the built-in drive motor velocity PID.");
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
|
||||||
|
|
||||||
|
drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
mode = Mode.TUNING_MODE;
|
||||||
|
|
||||||
|
NanoClock clock = NanoClock.system();
|
||||||
|
|
||||||
|
telemetry.addLine("Ready!");
|
||||||
|
telemetry.update();
|
||||||
|
telemetry.clearAll();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
boolean movingForwards = true;
|
||||||
|
MotionProfile activeProfile = generateProfile(true);
|
||||||
|
double profileStart = clock.seconds();
|
||||||
|
|
||||||
|
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
telemetry.addData("mode", mode);
|
||||||
|
|
||||||
|
switch (mode) {
|
||||||
|
case TUNING_MODE:
|
||||||
|
if (gamepad1.y) {
|
||||||
|
mode = Mode.DRIVER_MODE;
|
||||||
|
}
|
||||||
|
|
||||||
|
// calculate and set the motor power
|
||||||
|
double profileTime = clock.seconds() - profileStart;
|
||||||
|
|
||||||
|
if (profileTime > activeProfile.duration()) {
|
||||||
|
// generate a new profile
|
||||||
|
movingForwards = !movingForwards;
|
||||||
|
activeProfile = generateProfile(movingForwards);
|
||||||
|
profileStart = clock.seconds();
|
||||||
|
}
|
||||||
|
|
||||||
|
MotionState motionState = activeProfile.get(profileTime);
|
||||||
|
double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
|
||||||
|
|
||||||
|
drive.setDrivePower(new Pose2d(targetPower, 0, 0));
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
|
||||||
|
double currentVelo = poseVelo.getX();
|
||||||
|
|
||||||
|
// update telemetry
|
||||||
|
telemetry.addData("targetVelocity", motionState.getV());
|
||||||
|
telemetry.addData("measuredVelocity", currentVelo);
|
||||||
|
telemetry.addData("error", motionState.getV() - currentVelo);
|
||||||
|
break;
|
||||||
|
case DRIVER_MODE:
|
||||||
|
if (gamepad1.b) {
|
||||||
|
mode = Mode.TUNING_MODE;
|
||||||
|
movingForwards = true;
|
||||||
|
activeProfile = generateProfile(movingForwards);
|
||||||
|
profileStart = clock.seconds();
|
||||||
|
}
|
||||||
|
|
||||||
|
drive.setWeightedDrivePower(
|
||||||
|
new Pose2d(
|
||||||
|
-gamepad1.left_stick_y,
|
||||||
|
-gamepad1.left_stick_x,
|
||||||
|
-gamepad1.right_stick_x
|
||||||
|
)
|
||||||
|
);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,70 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
import java.util.Objects;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
|
||||||
|
* <p>
|
||||||
|
* Upon pressing start, your bot will turn at max power for RUNTIME seconds.
|
||||||
|
* <p>
|
||||||
|
* Further fine tuning of MAX_ANG_VEL may be desired.
|
||||||
|
*/
|
||||||
|
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class MaxAngularVeloTuner extends LinearOpMode {
|
||||||
|
public static double RUNTIME = 4.0;
|
||||||
|
|
||||||
|
private ElapsedTime timer;
|
||||||
|
private double maxAngVelocity = 0.0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
|
||||||
|
telemetry.addLine("Please ensure you have enough space cleared.");
|
||||||
|
telemetry.addLine("");
|
||||||
|
telemetry.addLine("Press start when ready.");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
drive.setDrivePower(new Pose2d(0, 0, 1));
|
||||||
|
timer = new ElapsedTime();
|
||||||
|
|
||||||
|
while (!isStopRequested() && timer.seconds() < RUNTIME) {
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
|
||||||
|
|
||||||
|
maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
|
||||||
|
}
|
||||||
|
|
||||||
|
drive.setDrivePower(new Pose2d());
|
||||||
|
|
||||||
|
telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
|
||||||
|
telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
while (!isStopRequested()) idle();
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,82 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.VoltageSensor;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
import java.util.Objects;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This routine is designed to calculate the maximum velocity your bot can achieve under load. It
|
||||||
|
* will also calculate the effective kF value for your velocity PID.
|
||||||
|
* <p>
|
||||||
|
* Upon pressing start, your bot will run at max power for RUNTIME seconds.
|
||||||
|
* <p>
|
||||||
|
* Further fine tuning of kF may be desired.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class MaxVelocityTuner extends LinearOpMode {
|
||||||
|
public static double RUNTIME = 2.0;
|
||||||
|
|
||||||
|
private ElapsedTime timer;
|
||||||
|
private double maxVelocity = 0.0;
|
||||||
|
|
||||||
|
private VoltageSensor batteryVoltageSensor;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
|
||||||
|
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
|
||||||
|
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
|
||||||
|
telemetry.addLine("Please ensure you have enough space cleared.");
|
||||||
|
telemetry.addLine("");
|
||||||
|
telemetry.addLine("Press start when ready.");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
drive.setDrivePower(new Pose2d(1, 0, 0));
|
||||||
|
timer = new ElapsedTime();
|
||||||
|
|
||||||
|
while (!isStopRequested() && timer.seconds() < RUNTIME) {
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
|
||||||
|
|
||||||
|
maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
|
||||||
|
}
|
||||||
|
|
||||||
|
drive.setDrivePower(new Pose2d());
|
||||||
|
|
||||||
|
double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
|
||||||
|
|
||||||
|
telemetry.addData("Max Velocity", maxVelocity);
|
||||||
|
telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
while (!isStopRequested() && opModeIsActive()) idle();
|
||||||
|
}
|
||||||
|
|
||||||
|
private double veloInchesToTicks(double inchesPerSec) {
|
||||||
|
return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,93 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This is a simple teleop routine for debugging your motor configuration.
|
||||||
|
* Pressing each of the buttons will power its respective motor.
|
||||||
|
*
|
||||||
|
* Button Mappings:
|
||||||
|
*
|
||||||
|
* Xbox/PS4 Button - Motor
|
||||||
|
* X / ▢ - Front Left
|
||||||
|
* Y / Δ - Front Right
|
||||||
|
* B / O - Rear Right
|
||||||
|
* A / X - Rear Left
|
||||||
|
* The buttons are mapped to match the wheels spatially if you
|
||||||
|
* were to rotate the gamepad 45deg°. x/square is the front left
|
||||||
|
* ________ and each button corresponds to the wheel as you go clockwise
|
||||||
|
* / ______ \
|
||||||
|
* ------------.-' _ '-..+ Front of Bot
|
||||||
|
* / _ ( Y ) _ \ ^
|
||||||
|
* | ( X ) _ ( B ) | Front Left \ Front Right
|
||||||
|
* ___ '. ( A ) /| Wheel \ Wheel
|
||||||
|
* .' '. '-._____.-' .' (x/▢) \ (Y/Δ)
|
||||||
|
* | | | \
|
||||||
|
* '.___.' '. | Rear Left \ Rear Right
|
||||||
|
* '. / Wheel \ Wheel
|
||||||
|
* \. .' (A/X) \ (B/O)
|
||||||
|
* \________/
|
||||||
|
*
|
||||||
|
* Uncomment the @Disabled tag below to use this opmode.
|
||||||
|
*/
|
||||||
|
@Disabled
|
||||||
|
@Config
|
||||||
|
@TeleOp(group = "drive")
|
||||||
|
public class MotorDirectionDebugger extends LinearOpMode {
|
||||||
|
public static double MOTOR_POWER = 0.7;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
telemetry.addLine("Press play to begin the debugging opmode");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
|
||||||
|
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
telemetry.addLine("Press each button to turn on its respective motor");
|
||||||
|
telemetry.addLine();
|
||||||
|
telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
|
||||||
|
telemetry.addLine("<font face=\"monospace\"> X / ▢ - Front Left</font>");
|
||||||
|
telemetry.addLine("<font face=\"monospace\"> Y / Δ - Front Right</font>");
|
||||||
|
telemetry.addLine("<font face=\"monospace\"> B / O - Rear Right</font>");
|
||||||
|
telemetry.addLine("<font face=\"monospace\"> A / X - Rear Left</font>");
|
||||||
|
telemetry.addLine();
|
||||||
|
|
||||||
|
if(gamepad1.x) {
|
||||||
|
drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
|
||||||
|
telemetry.addLine("Running Motor: Front Left");
|
||||||
|
} else if(gamepad1.y) {
|
||||||
|
drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
|
||||||
|
telemetry.addLine("Running Motor: Front Right");
|
||||||
|
} else if(gamepad1.b) {
|
||||||
|
drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
|
||||||
|
telemetry.addLine("Running Motor: Rear Right");
|
||||||
|
} else if(gamepad1.a) {
|
||||||
|
drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
|
||||||
|
telemetry.addLine("Running Motor: Rear Left");
|
||||||
|
} else {
|
||||||
|
drive.setMotorPowers(0, 0, 0, 0);
|
||||||
|
telemetry.addLine("Running Motor: None");
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,38 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Vector2d;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This is an example of a more complex path to really test the tuning.
|
||||||
|
*/
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class SplineTest extends LinearOpMode {
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
Trajectory traj = drive.trajectoryBuilder(new Pose2d())
|
||||||
|
.splineTo(new Vector2d(30, 30), 0)
|
||||||
|
.build();
|
||||||
|
|
||||||
|
drive.followTrajectory(traj);
|
||||||
|
|
||||||
|
sleep(2000);
|
||||||
|
|
||||||
|
drive.followTrajectory(
|
||||||
|
drive.trajectoryBuilder(traj.end(), true)
|
||||||
|
.splineTo(new Vector2d(0, 0), Math.toRadians(180))
|
||||||
|
.build()
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,45 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This is a simple routine to test translational drive capabilities.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class StrafeTest extends LinearOpMode {
|
||||||
|
public static double DISTANCE = 60; // in
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
|
||||||
|
.strafeRight(DISTANCE)
|
||||||
|
.build();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
drive.followTrajectory(trajectory);
|
||||||
|
|
||||||
|
Pose2d poseEstimate = drive.getPoseEstimate();
|
||||||
|
telemetry.addData("finalX", poseEstimate.getX());
|
||||||
|
telemetry.addData("finalY", poseEstimate.getY());
|
||||||
|
telemetry.addData("finalHeading", poseEstimate.getHeading());
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
while (!isStopRequested() && opModeIsActive()) ;
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,45 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This is a simple routine to test translational drive capabilities.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class StraightTest extends LinearOpMode {
|
||||||
|
public static double DISTANCE = 60; // in
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
|
||||||
|
.forward(DISTANCE)
|
||||||
|
.build();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
drive.followTrajectory(trajectory);
|
||||||
|
|
||||||
|
Pose2d poseEstimate = drive.getPoseEstimate();
|
||||||
|
telemetry.addData("finalX", poseEstimate.getX());
|
||||||
|
telemetry.addData("finalY", poseEstimate.getY());
|
||||||
|
telemetry.addData("finalHeading", poseEstimate.getHeading());
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
while (!isStopRequested() && opModeIsActive()) ;
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,87 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.util.Angle;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.util.MovingStatistics;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.internal.system.Misc;
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This routine determines the effective track width. The procedure works by executing a point turn
|
||||||
|
* with a given angle and measuring the difference between that angle and the actual angle (as
|
||||||
|
* indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
|
||||||
|
* given angle / actual angle gives a multiplicative adjustment to the estimated track width
|
||||||
|
* (effective track width = estimated track width * given angle / actual angle). The routine repeats
|
||||||
|
* this procedure a few times and averages the values for additional accuracy. Note: a relatively
|
||||||
|
* accurate track width estimate is important or else the angular constraints will be thrown off.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class TrackWidthTuner extends LinearOpMode {
|
||||||
|
public static double ANGLE = 180; // deg
|
||||||
|
public static int NUM_TRIALS = 5;
|
||||||
|
public static int DELAY = 1000; // ms
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
// TODO: if you haven't already, set the localizer to something that doesn't depend on
|
||||||
|
// drive encoders for computing the heading
|
||||||
|
|
||||||
|
telemetry.addLine("Press play to begin the track width tuner routine");
|
||||||
|
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Running...");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
|
||||||
|
for (int i = 0; i < NUM_TRIALS; i++) {
|
||||||
|
drive.setPoseEstimate(new Pose2d());
|
||||||
|
|
||||||
|
// it is important to handle heading wraparounds
|
||||||
|
double headingAccumulator = 0;
|
||||||
|
double lastHeading = 0;
|
||||||
|
|
||||||
|
drive.turnAsync(Math.toRadians(ANGLE));
|
||||||
|
|
||||||
|
while (!isStopRequested() && drive.isBusy()) {
|
||||||
|
double heading = drive.getPoseEstimate().getHeading();
|
||||||
|
headingAccumulator += Angle.norm(heading - lastHeading);
|
||||||
|
lastHeading = heading;
|
||||||
|
|
||||||
|
drive.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
|
||||||
|
trackWidthStats.add(trackWidth);
|
||||||
|
|
||||||
|
sleep(DELAY);
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Tuning complete");
|
||||||
|
telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
|
||||||
|
trackWidthStats.getMean(),
|
||||||
|
trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,103 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.util.Angle;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.util.MovingStatistics;
|
||||||
|
import com.qualcomm.robotcore.util.RobotLog;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.internal.system.Misc;
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This routine determines the effective forward offset for the lateral tracking wheel.
|
||||||
|
* The procedure executes a point turn at a given angle for a certain number of trials,
|
||||||
|
* along with a specified delay in milliseconds. The purpose of this is to track the
|
||||||
|
* change in the y position during the turn. The offset, or distance, of the lateral tracking
|
||||||
|
* wheel from the center or rotation allows the wheel to spin during a point turn, leading
|
||||||
|
* to an incorrect measurement for the y position. This creates an arc around around
|
||||||
|
* the center of rotation with an arc length of change in y and a radius equal to the forward
|
||||||
|
* offset. We can compute this offset by calculating (change in y position) / (change in heading)
|
||||||
|
* which returns the radius if the angle (change in heading) is in radians. This is based
|
||||||
|
* on the arc length formula of length = theta * radius.
|
||||||
|
*
|
||||||
|
* To run this routine, simply adjust the desired angle and specify the number of trials
|
||||||
|
* and the desired delay. Then, run the procedure. Once it finishes, it will print the
|
||||||
|
* average of all the calculated forward offsets derived from the calculation. This calculated
|
||||||
|
* forward offset is then added onto the current forward offset to produce an overall estimate
|
||||||
|
* for the forward offset. You can run this procedure as many times as necessary until a
|
||||||
|
* satisfactory result is produced.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group="drive")
|
||||||
|
public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
|
||||||
|
public static double ANGLE = 180; // deg
|
||||||
|
public static int NUM_TRIALS = 5;
|
||||||
|
public static int DELAY = 1000; // ms
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
|
||||||
|
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
|
||||||
|
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
|
||||||
|
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.addLine("Press play to begin the forward offset tuner");
|
||||||
|
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Running...");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
|
||||||
|
for (int i = 0; i < NUM_TRIALS; i++) {
|
||||||
|
drive.setPoseEstimate(new Pose2d());
|
||||||
|
|
||||||
|
// it is important to handle heading wraparounds
|
||||||
|
double headingAccumulator = 0;
|
||||||
|
double lastHeading = 0;
|
||||||
|
|
||||||
|
drive.turnAsync(Math.toRadians(ANGLE));
|
||||||
|
|
||||||
|
while (!isStopRequested() && drive.isBusy()) {
|
||||||
|
double heading = drive.getPoseEstimate().getHeading();
|
||||||
|
headingAccumulator += Angle.norm(heading - lastHeading);
|
||||||
|
lastHeading = heading;
|
||||||
|
|
||||||
|
drive.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
|
||||||
|
drive.getPoseEstimate().getY() / headingAccumulator;
|
||||||
|
forwardOffsetStats.add(forwardOffset);
|
||||||
|
|
||||||
|
sleep(DELAY);
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Tuning complete");
|
||||||
|
telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
|
||||||
|
forwardOffsetStats.getMean(),
|
||||||
|
forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
idle();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,130 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.util.Angle;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
import com.qualcomm.robotcore.util.RobotLog;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
|
||||||
|
* LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
|
||||||
|
* wheels.
|
||||||
|
*
|
||||||
|
* Tuning Routine:
|
||||||
|
*
|
||||||
|
* 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
|
||||||
|
* measured value. This need only be an estimated value as you will be tuning it anyways.
|
||||||
|
*
|
||||||
|
* 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
|
||||||
|
* similar mark right below the indicator on your bot. This will be your reference point to
|
||||||
|
* ensure you've turned exactly 360°.
|
||||||
|
*
|
||||||
|
* 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
|
||||||
|
* identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
|
||||||
|
* connect your computer to the RC's WiFi network. In your browser, navigate to
|
||||||
|
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
|
||||||
|
* if you are using the Control Hub.
|
||||||
|
* Ensure the field is showing (select the field view in top right of the page).
|
||||||
|
*
|
||||||
|
* 4. Press play to begin the tuning routine.
|
||||||
|
*
|
||||||
|
* 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
|
||||||
|
*
|
||||||
|
* 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
|
||||||
|
*
|
||||||
|
* 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
|
||||||
|
* on the bot and on the ground you created earlier should be lined up.
|
||||||
|
*
|
||||||
|
* 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
|
||||||
|
* StandardTrackingWheelLocalizer.java class.
|
||||||
|
*
|
||||||
|
* 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
|
||||||
|
* yourself. Read the heading output and follow the advice stated in the note below to manually
|
||||||
|
* nudge the values yourself.
|
||||||
|
*
|
||||||
|
* Note:
|
||||||
|
* It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
|
||||||
|
* a line from the circumference to the center should be present, representing the bot. The line
|
||||||
|
* indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
|
||||||
|
* dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
|
||||||
|
* the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
|
||||||
|
* actual bot, the LATERAL_DISTANCE should be increased.
|
||||||
|
*
|
||||||
|
* If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
|
||||||
|
* position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
|
||||||
|
* effective center of rotation. You can ignore this effect. The center of rotation will be offset
|
||||||
|
* slightly but your heading will still be fine. This does not affect your overall tracking
|
||||||
|
* precision. The heading should still line up.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@TeleOp(group = "drive")
|
||||||
|
public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
|
||||||
|
public static int NUM_TURNS = 10;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
|
||||||
|
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
|
||||||
|
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
|
||||||
|
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.addLine("Prior to beginning the routine, please read the directions "
|
||||||
|
+ "located in the comments of the opmode file.");
|
||||||
|
telemetry.addLine("Press play to begin the tuning routine.");
|
||||||
|
telemetry.addLine("");
|
||||||
|
telemetry.addLine("Press Y/△ to stop the routine.");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
double headingAccumulator = 0;
|
||||||
|
double lastHeading = 0;
|
||||||
|
|
||||||
|
boolean tuningFinished = false;
|
||||||
|
|
||||||
|
while (!isStopRequested() && !tuningFinished) {
|
||||||
|
Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
|
||||||
|
drive.setDrivePower(vel);
|
||||||
|
|
||||||
|
drive.update();
|
||||||
|
|
||||||
|
double heading = drive.getPoseEstimate().getHeading();
|
||||||
|
double deltaHeading = heading - lastHeading;
|
||||||
|
|
||||||
|
headingAccumulator += Angle.normDelta(deltaHeading);
|
||||||
|
lastHeading = heading;
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
|
||||||
|
telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
|
||||||
|
telemetry.addLine();
|
||||||
|
telemetry.addLine("Press Y/△ to conclude routine");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
if (gamepad1.y)
|
||||||
|
tuningFinished = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetry.clearAll();
|
||||||
|
telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
|
||||||
|
telemetry.addLine("Effective LATERAL_DISTANCE: " +
|
||||||
|
(headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
|
||||||
|
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
while (!isStopRequested()) idle();
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,27 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This is a simple routine to test turning capabilities.
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(group = "drive")
|
||||||
|
public class TurnTest extends LinearOpMode {
|
||||||
|
public static double ANGLE = 90; // deg
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
drive.turn(Math.toRadians(ANGLE));
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,4 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.trajectorysequence;
|
||||||
|
|
||||||
|
|
||||||
|
public class EmptySequenceException extends RuntimeException { }
|
|
@ -0,0 +1,44 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.trajectorysequence;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment;
|
||||||
|
|
||||||
|
import java.util.Collections;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
public class TrajectorySequence {
|
||||||
|
private final List<SequenceSegment> sequenceList;
|
||||||
|
|
||||||
|
public TrajectorySequence(List<SequenceSegment> sequenceList) {
|
||||||
|
if (sequenceList.size() == 0) throw new EmptySequenceException();
|
||||||
|
|
||||||
|
this.sequenceList = Collections.unmodifiableList(sequenceList);
|
||||||
|
}
|
||||||
|
|
||||||
|
public Pose2d start() {
|
||||||
|
return sequenceList.get(0).getStartPose();
|
||||||
|
}
|
||||||
|
|
||||||
|
public Pose2d end() {
|
||||||
|
return sequenceList.get(sequenceList.size() - 1).getEndPose();
|
||||||
|
}
|
||||||
|
|
||||||
|
public double duration() {
|
||||||
|
double total = 0.0;
|
||||||
|
|
||||||
|
for (SequenceSegment segment : sequenceList) {
|
||||||
|
total += segment.getDuration();
|
||||||
|
}
|
||||||
|
|
||||||
|
return total;
|
||||||
|
}
|
||||||
|
|
||||||
|
public SequenceSegment get(int i) {
|
||||||
|
return sequenceList.get(i);
|
||||||
|
}
|
||||||
|
|
||||||
|
public int size() {
|
||||||
|
return sequenceList.size();
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,711 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.trajectorysequence;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Vector2d;
|
||||||
|
import com.acmerobotics.roadrunner.path.PathContinuityViolationException;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionProfile;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionState;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.DisplacementMarker;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.DisplacementProducer;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.MarkerCallback;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.SpatialMarker;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TemporalMarker;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TimeProducer;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
|
||||||
|
import com.acmerobotics.roadrunner.util.Angle;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment;
|
||||||
|
|
||||||
|
import java.util.ArrayList;
|
||||||
|
import java.util.Collections;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
public class TrajectorySequenceBuilder {
|
||||||
|
private final double resolution = 0.25;
|
||||||
|
|
||||||
|
private final TrajectoryVelocityConstraint baseVelConstraint;
|
||||||
|
private final TrajectoryAccelerationConstraint baseAccelConstraint;
|
||||||
|
|
||||||
|
private TrajectoryVelocityConstraint currentVelConstraint;
|
||||||
|
private TrajectoryAccelerationConstraint currentAccelConstraint;
|
||||||
|
|
||||||
|
private final double baseTurnConstraintMaxAngVel;
|
||||||
|
private final double baseTurnConstraintMaxAngAccel;
|
||||||
|
|
||||||
|
private double currentTurnConstraintMaxAngVel;
|
||||||
|
private double currentTurnConstraintMaxAngAccel;
|
||||||
|
|
||||||
|
private final List<SequenceSegment> sequenceSegments;
|
||||||
|
|
||||||
|
private final List<TemporalMarker> temporalMarkers;
|
||||||
|
private final List<DisplacementMarker> displacementMarkers;
|
||||||
|
private final List<SpatialMarker> spatialMarkers;
|
||||||
|
|
||||||
|
private Pose2d lastPose;
|
||||||
|
|
||||||
|
private double tangentOffset;
|
||||||
|
|
||||||
|
private boolean setAbsoluteTangent;
|
||||||
|
private double absoluteTangent;
|
||||||
|
|
||||||
|
private TrajectoryBuilder currentTrajectoryBuilder;
|
||||||
|
|
||||||
|
private double currentDuration;
|
||||||
|
private double currentDisplacement;
|
||||||
|
|
||||||
|
private double lastDurationTraj;
|
||||||
|
private double lastDisplacementTraj;
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder(
|
||||||
|
Pose2d startPose,
|
||||||
|
Double startTangent,
|
||||||
|
TrajectoryVelocityConstraint baseVelConstraint,
|
||||||
|
TrajectoryAccelerationConstraint baseAccelConstraint,
|
||||||
|
double baseTurnConstraintMaxAngVel,
|
||||||
|
double baseTurnConstraintMaxAngAccel
|
||||||
|
) {
|
||||||
|
this.baseVelConstraint = baseVelConstraint;
|
||||||
|
this.baseAccelConstraint = baseAccelConstraint;
|
||||||
|
|
||||||
|
this.currentVelConstraint = baseVelConstraint;
|
||||||
|
this.currentAccelConstraint = baseAccelConstraint;
|
||||||
|
|
||||||
|
this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
|
||||||
|
this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
|
||||||
|
|
||||||
|
this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
|
||||||
|
this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
|
||||||
|
|
||||||
|
sequenceSegments = new ArrayList<>();
|
||||||
|
|
||||||
|
temporalMarkers = new ArrayList<>();
|
||||||
|
displacementMarkers = new ArrayList<>();
|
||||||
|
spatialMarkers = new ArrayList<>();
|
||||||
|
|
||||||
|
lastPose = startPose;
|
||||||
|
|
||||||
|
tangentOffset = 0.0;
|
||||||
|
|
||||||
|
setAbsoluteTangent = (startTangent != null);
|
||||||
|
absoluteTangent = startTangent != null ? startTangent : 0.0;
|
||||||
|
|
||||||
|
currentTrajectoryBuilder = null;
|
||||||
|
|
||||||
|
currentDuration = 0.0;
|
||||||
|
currentDisplacement = 0.0;
|
||||||
|
|
||||||
|
lastDurationTraj = 0.0;
|
||||||
|
lastDisplacementTraj = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder(
|
||||||
|
Pose2d startPose,
|
||||||
|
TrajectoryVelocityConstraint baseVelConstraint,
|
||||||
|
TrajectoryAccelerationConstraint baseAccelConstraint,
|
||||||
|
double baseTurnConstraintMaxAngVel,
|
||||||
|
double baseTurnConstraintMaxAngAccel
|
||||||
|
) {
|
||||||
|
this(
|
||||||
|
startPose, null,
|
||||||
|
baseVelConstraint, baseAccelConstraint,
|
||||||
|
baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder lineTo(Vector2d endPosition) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder lineTo(
|
||||||
|
Vector2d endPosition,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder lineToConstantHeading(
|
||||||
|
Vector2d endPosition,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder lineToLinearHeading(
|
||||||
|
Pose2d endPose,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder lineToSplineHeading(
|
||||||
|
Pose2d endPose,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder strafeTo(
|
||||||
|
Vector2d endPosition,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder forward(double distance) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder forward(
|
||||||
|
double distance,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder back(double distance) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder back(
|
||||||
|
double distance,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder strafeLeft(double distance) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder strafeLeft(
|
||||||
|
double distance,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder strafeRight(double distance) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder strafeRight(
|
||||||
|
double distance,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder splineTo(
|
||||||
|
Vector2d endPosition,
|
||||||
|
double endHeading,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder splineToConstantHeading(
|
||||||
|
Vector2d endPosition,
|
||||||
|
double endHeading,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder splineToLinearHeading(
|
||||||
|
Pose2d endPose,
|
||||||
|
double endHeading,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder splineToSplineHeading(
|
||||||
|
Pose2d endPose,
|
||||||
|
double endHeading,
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint));
|
||||||
|
}
|
||||||
|
|
||||||
|
private TrajectorySequenceBuilder addPath(AddPathCallback callback) {
|
||||||
|
if (currentTrajectoryBuilder == null) newPath();
|
||||||
|
|
||||||
|
try {
|
||||||
|
callback.run();
|
||||||
|
} catch (PathContinuityViolationException e) {
|
||||||
|
newPath();
|
||||||
|
callback.run();
|
||||||
|
}
|
||||||
|
|
||||||
|
Trajectory builtTraj = currentTrajectoryBuilder.build();
|
||||||
|
|
||||||
|
double durationDifference = builtTraj.duration() - lastDurationTraj;
|
||||||
|
double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj;
|
||||||
|
|
||||||
|
lastPose = builtTraj.end();
|
||||||
|
currentDuration += durationDifference;
|
||||||
|
currentDisplacement += displacementDifference;
|
||||||
|
|
||||||
|
lastDurationTraj = builtTraj.duration();
|
||||||
|
lastDisplacementTraj = builtTraj.getPath().length();
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder setTangent(double tangent) {
|
||||||
|
setAbsoluteTangent = true;
|
||||||
|
absoluteTangent = tangent;
|
||||||
|
|
||||||
|
pushPath();
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
private TrajectorySequenceBuilder setTangentOffset(double offset) {
|
||||||
|
setAbsoluteTangent = false;
|
||||||
|
|
||||||
|
this.tangentOffset = offset;
|
||||||
|
this.pushPath();
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder setReversed(boolean reversed) {
|
||||||
|
return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder setConstraints(
|
||||||
|
TrajectoryVelocityConstraint velConstraint,
|
||||||
|
TrajectoryAccelerationConstraint accelConstraint
|
||||||
|
) {
|
||||||
|
this.currentVelConstraint = velConstraint;
|
||||||
|
this.currentAccelConstraint = accelConstraint;
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder resetConstraints() {
|
||||||
|
this.currentVelConstraint = this.baseVelConstraint;
|
||||||
|
this.currentAccelConstraint = this.baseAccelConstraint;
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) {
|
||||||
|
this.currentVelConstraint = velConstraint;
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder resetVelConstraint() {
|
||||||
|
this.currentVelConstraint = this.baseVelConstraint;
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) {
|
||||||
|
this.currentAccelConstraint = accelConstraint;
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder resetAccelConstraint() {
|
||||||
|
this.currentAccelConstraint = this.baseAccelConstraint;
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) {
|
||||||
|
this.currentTurnConstraintMaxAngVel = maxAngVel;
|
||||||
|
this.currentTurnConstraintMaxAngAccel = maxAngAccel;
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder resetTurnConstraint() {
|
||||||
|
this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
|
||||||
|
this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) {
|
||||||
|
return this.addTemporalMarker(currentDuration, callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) {
|
||||||
|
return this.addTemporalMarker(currentDuration + offset, callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) {
|
||||||
|
return this.addTemporalMarker(0.0, time, callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) {
|
||||||
|
return this.addTemporalMarker(time -> scale * time + offset, callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) {
|
||||||
|
this.temporalMarkers.add(new TemporalMarker(time, callback));
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) {
|
||||||
|
this.spatialMarkers.add(new SpatialMarker(point, callback));
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) {
|
||||||
|
return this.addDisplacementMarker(currentDisplacement, callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) {
|
||||||
|
return this.addDisplacementMarker(currentDisplacement + offset, callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) {
|
||||||
|
return this.addDisplacementMarker(0.0, displacement, callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) {
|
||||||
|
return addDisplacementMarker((displacement -> scale * displacement + offset), callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) {
|
||||||
|
displacementMarkers.add(new DisplacementMarker(displacement, callback));
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder turn(double angle) {
|
||||||
|
return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) {
|
||||||
|
pushPath();
|
||||||
|
|
||||||
|
MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile(
|
||||||
|
new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0),
|
||||||
|
new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0),
|
||||||
|
maxAngVel,
|
||||||
|
maxAngAccel
|
||||||
|
);
|
||||||
|
|
||||||
|
sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList()));
|
||||||
|
|
||||||
|
lastPose = new Pose2d(
|
||||||
|
lastPose.getX(), lastPose.getY(),
|
||||||
|
Angle.norm(lastPose.getHeading() + angle)
|
||||||
|
);
|
||||||
|
|
||||||
|
currentDuration += turnProfile.duration();
|
||||||
|
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder waitSeconds(double seconds) {
|
||||||
|
pushPath();
|
||||||
|
sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));
|
||||||
|
|
||||||
|
currentDuration += seconds;
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) {
|
||||||
|
pushPath();
|
||||||
|
|
||||||
|
sequenceSegments.add(new TrajectorySegment(trajectory));
|
||||||
|
return this;
|
||||||
|
}
|
||||||
|
|
||||||
|
private void pushPath() {
|
||||||
|
if (currentTrajectoryBuilder != null) {
|
||||||
|
Trajectory builtTraj = currentTrajectoryBuilder.build();
|
||||||
|
sequenceSegments.add(new TrajectorySegment(builtTraj));
|
||||||
|
}
|
||||||
|
|
||||||
|
currentTrajectoryBuilder = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
private void newPath() {
|
||||||
|
if (currentTrajectoryBuilder != null)
|
||||||
|
pushPath();
|
||||||
|
|
||||||
|
lastDurationTraj = 0.0;
|
||||||
|
lastDisplacementTraj = 0.0;
|
||||||
|
|
||||||
|
double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset);
|
||||||
|
|
||||||
|
currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution);
|
||||||
|
}
|
||||||
|
|
||||||
|
public TrajectorySequence build() {
|
||||||
|
pushPath();
|
||||||
|
|
||||||
|
List<TrajectoryMarker> globalMarkers = convertMarkersToGlobal(
|
||||||
|
sequenceSegments,
|
||||||
|
temporalMarkers, displacementMarkers, spatialMarkers
|
||||||
|
);
|
||||||
|
|
||||||
|
return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments));
|
||||||
|
}
|
||||||
|
|
||||||
|
private List<TrajectoryMarker> convertMarkersToGlobal(
|
||||||
|
List<SequenceSegment> sequenceSegments,
|
||||||
|
List<TemporalMarker> temporalMarkers,
|
||||||
|
List<DisplacementMarker> displacementMarkers,
|
||||||
|
List<SpatialMarker> spatialMarkers
|
||||||
|
) {
|
||||||
|
ArrayList<TrajectoryMarker> trajectoryMarkers = new ArrayList<>();
|
||||||
|
|
||||||
|
// Convert temporal markers
|
||||||
|
for (TemporalMarker marker : temporalMarkers) {
|
||||||
|
trajectoryMarkers.add(
|
||||||
|
new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback())
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Convert displacement markers
|
||||||
|
for (DisplacementMarker marker : displacementMarkers) {
|
||||||
|
double time = displacementToTime(
|
||||||
|
sequenceSegments,
|
||||||
|
marker.getProducer().produce(currentDisplacement)
|
||||||
|
);
|
||||||
|
|
||||||
|
trajectoryMarkers.add(
|
||||||
|
new TrajectoryMarker(
|
||||||
|
time,
|
||||||
|
marker.getCallback()
|
||||||
|
)
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Convert spatial markers
|
||||||
|
for (SpatialMarker marker : spatialMarkers) {
|
||||||
|
trajectoryMarkers.add(
|
||||||
|
new TrajectoryMarker(
|
||||||
|
pointToTime(sequenceSegments, marker.getPoint()),
|
||||||
|
marker.getCallback()
|
||||||
|
)
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
return trajectoryMarkers;
|
||||||
|
}
|
||||||
|
|
||||||
|
private List<SequenceSegment> projectGlobalMarkersToLocalSegments(List<TrajectoryMarker> markers, List<SequenceSegment> sequenceSegments) {
|
||||||
|
if (sequenceSegments.isEmpty()) return Collections.emptyList();
|
||||||
|
|
||||||
|
double totalSequenceDuration = 0;
|
||||||
|
for (SequenceSegment segment : sequenceSegments) {
|
||||||
|
totalSequenceDuration += segment.getDuration();
|
||||||
|
}
|
||||||
|
|
||||||
|
for (TrajectoryMarker marker : markers) {
|
||||||
|
SequenceSegment segment = null;
|
||||||
|
int segmentIndex = 0;
|
||||||
|
double segmentOffsetTime = 0;
|
||||||
|
|
||||||
|
double currentTime = 0;
|
||||||
|
for (int i = 0; i < sequenceSegments.size(); i++) {
|
||||||
|
SequenceSegment seg = sequenceSegments.get(i);
|
||||||
|
|
||||||
|
double markerTime = Math.min(marker.getTime(), totalSequenceDuration);
|
||||||
|
|
||||||
|
if (currentTime + seg.getDuration() >= markerTime) {
|
||||||
|
segment = seg;
|
||||||
|
segmentIndex = i;
|
||||||
|
segmentOffsetTime = markerTime - currentTime;
|
||||||
|
|
||||||
|
break;
|
||||||
|
} else {
|
||||||
|
currentTime += seg.getDuration();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
SequenceSegment newSegment = null;
|
||||||
|
|
||||||
|
if (segment instanceof WaitSegment) {
|
||||||
|
List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
|
||||||
|
|
||||||
|
newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
|
||||||
|
newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
|
||||||
|
|
||||||
|
WaitSegment thisSegment = (WaitSegment) segment;
|
||||||
|
newSegment = new WaitSegment(thisSegment.getStartPose(), thisSegment.getDuration(), newMarkers);
|
||||||
|
} else if (segment instanceof TurnSegment) {
|
||||||
|
List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
|
||||||
|
|
||||||
|
newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
|
||||||
|
newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
|
||||||
|
|
||||||
|
TurnSegment thisSegment = (TurnSegment) segment;
|
||||||
|
newSegment = new TurnSegment(thisSegment.getStartPose(), thisSegment.getTotalRotation(), thisSegment.getMotionProfile(), newMarkers);
|
||||||
|
} else if (segment instanceof TrajectorySegment) {
|
||||||
|
TrajectorySegment thisSegment = (TrajectorySegment) segment;
|
||||||
|
|
||||||
|
List<TrajectoryMarker> newMarkers = new ArrayList<>(thisSegment.getTrajectory().getMarkers());
|
||||||
|
newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
|
||||||
|
|
||||||
|
newSegment = new TrajectorySegment(new Trajectory(thisSegment.getTrajectory().getPath(), thisSegment.getTrajectory().getProfile(), newMarkers));
|
||||||
|
}
|
||||||
|
|
||||||
|
sequenceSegments.set(segmentIndex, newSegment);
|
||||||
|
}
|
||||||
|
|
||||||
|
return sequenceSegments;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private
|
||||||
|
// note: this assumes that the profile position is monotonic increasing
|
||||||
|
private Double motionProfileDisplacementToTime(MotionProfile profile, double s) {
|
||||||
|
double tLo = 0.0;
|
||||||
|
double tHi = profile.duration();
|
||||||
|
while (!(Math.abs(tLo - tHi) < 1e-6)) {
|
||||||
|
double tMid = 0.5 * (tLo + tHi);
|
||||||
|
if (profile.get(tMid).getX() > s) {
|
||||||
|
tHi = tMid;
|
||||||
|
} else {
|
||||||
|
tLo = tMid;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return 0.5 * (tLo + tHi);
|
||||||
|
}
|
||||||
|
|
||||||
|
private Double displacementToTime(List<SequenceSegment> sequenceSegments, double s) {
|
||||||
|
double currentTime = 0.0;
|
||||||
|
double currentDisplacement = 0.0;
|
||||||
|
|
||||||
|
for (SequenceSegment segment : sequenceSegments) {
|
||||||
|
if (segment instanceof TrajectorySegment) {
|
||||||
|
TrajectorySegment thisSegment = (TrajectorySegment) segment;
|
||||||
|
|
||||||
|
double segmentLength = thisSegment.getTrajectory().getPath().length();
|
||||||
|
|
||||||
|
if (currentDisplacement + segmentLength > s) {
|
||||||
|
double target = s - currentDisplacement;
|
||||||
|
double timeInSegment = motionProfileDisplacementToTime(
|
||||||
|
thisSegment.getTrajectory().getProfile(),
|
||||||
|
target
|
||||||
|
);
|
||||||
|
|
||||||
|
return currentTime + timeInSegment;
|
||||||
|
} else {
|
||||||
|
currentDisplacement += segmentLength;
|
||||||
|
currentTime += thisSegment.getTrajectory().duration();
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
currentTime += segment.getDuration();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
private Double pointToTime(List<SequenceSegment> sequenceSegments, Vector2d point) {
|
||||||
|
class ComparingPoints {
|
||||||
|
private final double distanceToPoint;
|
||||||
|
private final double totalDisplacement;
|
||||||
|
private final double thisPathDisplacement;
|
||||||
|
|
||||||
|
public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) {
|
||||||
|
this.distanceToPoint = distanceToPoint;
|
||||||
|
this.totalDisplacement = totalDisplacement;
|
||||||
|
this.thisPathDisplacement = thisPathDisplacement;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
List<ComparingPoints> projectedPoints = new ArrayList<>();
|
||||||
|
|
||||||
|
for (SequenceSegment segment : sequenceSegments) {
|
||||||
|
if (segment instanceof TrajectorySegment) {
|
||||||
|
TrajectorySegment thisSegment = (TrajectorySegment) segment;
|
||||||
|
|
||||||
|
double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25);
|
||||||
|
Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec();
|
||||||
|
double distanceToPoint = point.minus(projectedPoint).norm();
|
||||||
|
|
||||||
|
double totalDisplacement = 0.0;
|
||||||
|
|
||||||
|
for (ComparingPoints comparingPoint : projectedPoints) {
|
||||||
|
totalDisplacement += comparingPoint.totalDisplacement;
|
||||||
|
}
|
||||||
|
|
||||||
|
totalDisplacement += displacement;
|
||||||
|
|
||||||
|
projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ComparingPoints closestPoint = null;
|
||||||
|
|
||||||
|
for (ComparingPoints comparingPoint : projectedPoints) {
|
||||||
|
if (closestPoint == null) {
|
||||||
|
closestPoint = comparingPoint;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint)
|
||||||
|
closestPoint = comparingPoint;
|
||||||
|
}
|
||||||
|
|
||||||
|
return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement);
|
||||||
|
}
|
||||||
|
|
||||||
|
private interface AddPathCallback {
|
||||||
|
void run();
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,273 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.trajectorysequence;
|
||||||
|
|
||||||
|
import androidx.annotation.Nullable;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.canvas.Canvas;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||||
|
import com.acmerobotics.roadrunner.control.PIDCoefficients;
|
||||||
|
import com.acmerobotics.roadrunner.control.PIDFController;
|
||||||
|
import com.acmerobotics.roadrunner.drive.DriveSignal;
|
||||||
|
import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionState;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
|
||||||
|
import com.acmerobotics.roadrunner.util.NanoClock;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment;
|
||||||
|
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment;
|
||||||
|
import org.firstinspires.ftc.teamcode.util.DashboardUtil;
|
||||||
|
|
||||||
|
import java.util.ArrayList;
|
||||||
|
import java.util.Collections;
|
||||||
|
import java.util.LinkedList;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
@Config
|
||||||
|
public class TrajectorySequenceRunner {
|
||||||
|
public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a";
|
||||||
|
public static String COLOR_INACTIVE_TURN = "#7c4dff7a";
|
||||||
|
public static String COLOR_INACTIVE_WAIT = "#dd2c007a";
|
||||||
|
|
||||||
|
public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50";
|
||||||
|
public static String COLOR_ACTIVE_TURN = "#7c4dff";
|
||||||
|
public static String COLOR_ACTIVE_WAIT = "#dd2c00";
|
||||||
|
|
||||||
|
public static int POSE_HISTORY_LIMIT = 100;
|
||||||
|
|
||||||
|
private final TrajectoryFollower follower;
|
||||||
|
|
||||||
|
private final PIDFController turnController;
|
||||||
|
|
||||||
|
private final NanoClock clock;
|
||||||
|
|
||||||
|
private TrajectorySequence currentTrajectorySequence;
|
||||||
|
private double currentSegmentStartTime;
|
||||||
|
private int currentSegmentIndex;
|
||||||
|
private int lastSegmentIndex;
|
||||||
|
|
||||||
|
private Pose2d lastPoseError = new Pose2d();
|
||||||
|
|
||||||
|
List<TrajectoryMarker> remainingMarkers = new ArrayList<>();
|
||||||
|
|
||||||
|
private final FtcDashboard dashboard;
|
||||||
|
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
|
||||||
|
|
||||||
|
public TrajectorySequenceRunner(TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients) {
|
||||||
|
this.follower = follower;
|
||||||
|
|
||||||
|
turnController = new PIDFController(headingPIDCoefficients);
|
||||||
|
turnController.setInputBounds(0, 2 * Math.PI);
|
||||||
|
|
||||||
|
clock = NanoClock.system();
|
||||||
|
|
||||||
|
dashboard = FtcDashboard.getInstance();
|
||||||
|
dashboard.setTelemetryTransmissionInterval(25);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
|
||||||
|
currentTrajectorySequence = trajectorySequence;
|
||||||
|
currentSegmentStartTime = clock.seconds();
|
||||||
|
currentSegmentIndex = 0;
|
||||||
|
lastSegmentIndex = -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
public @Nullable
|
||||||
|
DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) {
|
||||||
|
Pose2d targetPose = null;
|
||||||
|
DriveSignal driveSignal = null;
|
||||||
|
|
||||||
|
TelemetryPacket packet = new TelemetryPacket();
|
||||||
|
Canvas fieldOverlay = packet.fieldOverlay();
|
||||||
|
|
||||||
|
SequenceSegment currentSegment = null;
|
||||||
|
|
||||||
|
if (currentTrajectorySequence != null) {
|
||||||
|
if (currentSegmentIndex >= currentTrajectorySequence.size()) {
|
||||||
|
for (TrajectoryMarker marker : remainingMarkers) {
|
||||||
|
marker.getCallback().onMarkerReached();
|
||||||
|
}
|
||||||
|
|
||||||
|
remainingMarkers.clear();
|
||||||
|
|
||||||
|
currentTrajectorySequence = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentTrajectorySequence == null)
|
||||||
|
return new DriveSignal();
|
||||||
|
|
||||||
|
double now = clock.seconds();
|
||||||
|
boolean isNewTransition = currentSegmentIndex != lastSegmentIndex;
|
||||||
|
|
||||||
|
currentSegment = currentTrajectorySequence.get(currentSegmentIndex);
|
||||||
|
|
||||||
|
if (isNewTransition) {
|
||||||
|
currentSegmentStartTime = now;
|
||||||
|
lastSegmentIndex = currentSegmentIndex;
|
||||||
|
|
||||||
|
for (TrajectoryMarker marker : remainingMarkers) {
|
||||||
|
marker.getCallback().onMarkerReached();
|
||||||
|
}
|
||||||
|
|
||||||
|
remainingMarkers.clear();
|
||||||
|
|
||||||
|
remainingMarkers.addAll(currentSegment.getMarkers());
|
||||||
|
Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime()));
|
||||||
|
}
|
||||||
|
|
||||||
|
double deltaTime = now - currentSegmentStartTime;
|
||||||
|
|
||||||
|
if (currentSegment instanceof TrajectorySegment) {
|
||||||
|
Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
|
||||||
|
|
||||||
|
if (isNewTransition)
|
||||||
|
follower.followTrajectory(currentTrajectory);
|
||||||
|
|
||||||
|
if (!follower.isFollowing()) {
|
||||||
|
currentSegmentIndex++;
|
||||||
|
|
||||||
|
driveSignal = new DriveSignal();
|
||||||
|
} else {
|
||||||
|
driveSignal = follower.update(poseEstimate, poseVelocity);
|
||||||
|
lastPoseError = follower.getLastError();
|
||||||
|
}
|
||||||
|
|
||||||
|
targetPose = currentTrajectory.get(deltaTime);
|
||||||
|
} else if (currentSegment instanceof TurnSegment) {
|
||||||
|
MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime);
|
||||||
|
|
||||||
|
turnController.setTargetPosition(targetState.getX());
|
||||||
|
|
||||||
|
double correction = turnController.update(poseEstimate.getHeading());
|
||||||
|
|
||||||
|
double targetOmega = targetState.getV();
|
||||||
|
double targetAlpha = targetState.getA();
|
||||||
|
|
||||||
|
lastPoseError = new Pose2d(0, 0, turnController.getLastError());
|
||||||
|
|
||||||
|
Pose2d startPose = currentSegment.getStartPose();
|
||||||
|
targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
|
||||||
|
|
||||||
|
driveSignal = new DriveSignal(
|
||||||
|
new Pose2d(0, 0, targetOmega + correction),
|
||||||
|
new Pose2d(0, 0, targetAlpha)
|
||||||
|
);
|
||||||
|
|
||||||
|
if (deltaTime >= currentSegment.getDuration()) {
|
||||||
|
currentSegmentIndex++;
|
||||||
|
driveSignal = new DriveSignal();
|
||||||
|
}
|
||||||
|
} else if (currentSegment instanceof WaitSegment) {
|
||||||
|
lastPoseError = new Pose2d();
|
||||||
|
|
||||||
|
targetPose = currentSegment.getStartPose();
|
||||||
|
driveSignal = new DriveSignal();
|
||||||
|
|
||||||
|
if (deltaTime >= currentSegment.getDuration()) {
|
||||||
|
currentSegmentIndex++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) {
|
||||||
|
remainingMarkers.get(0).getCallback().onMarkerReached();
|
||||||
|
remainingMarkers.remove(0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
poseHistory.add(poseEstimate);
|
||||||
|
|
||||||
|
if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) {
|
||||||
|
poseHistory.removeFirst();
|
||||||
|
}
|
||||||
|
|
||||||
|
packet.put("x", poseEstimate.getX());
|
||||||
|
packet.put("y", poseEstimate.getY());
|
||||||
|
packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading()));
|
||||||
|
|
||||||
|
packet.put("xError", getLastPoseError().getX());
|
||||||
|
packet.put("yError", getLastPoseError().getY());
|
||||||
|
packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading()));
|
||||||
|
|
||||||
|
draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate);
|
||||||
|
|
||||||
|
dashboard.sendTelemetryPacket(packet);
|
||||||
|
|
||||||
|
return driveSignal;
|
||||||
|
}
|
||||||
|
|
||||||
|
private void draw(
|
||||||
|
Canvas fieldOverlay,
|
||||||
|
TrajectorySequence sequence, SequenceSegment currentSegment,
|
||||||
|
Pose2d targetPose, Pose2d poseEstimate
|
||||||
|
) {
|
||||||
|
if (sequence != null) {
|
||||||
|
for (int i = 0; i < sequence.size(); i++) {
|
||||||
|
SequenceSegment segment = sequence.get(i);
|
||||||
|
|
||||||
|
if (segment instanceof TrajectorySegment) {
|
||||||
|
fieldOverlay.setStrokeWidth(1);
|
||||||
|
fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY);
|
||||||
|
|
||||||
|
DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath());
|
||||||
|
} else if (segment instanceof TurnSegment) {
|
||||||
|
Pose2d pose = segment.getStartPose();
|
||||||
|
|
||||||
|
fieldOverlay.setFill(COLOR_INACTIVE_TURN);
|
||||||
|
fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2);
|
||||||
|
} else if (segment instanceof WaitSegment) {
|
||||||
|
Pose2d pose = segment.getStartPose();
|
||||||
|
|
||||||
|
fieldOverlay.setStrokeWidth(1);
|
||||||
|
fieldOverlay.setStroke(COLOR_INACTIVE_WAIT);
|
||||||
|
fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentSegment != null) {
|
||||||
|
if (currentSegment instanceof TrajectorySegment) {
|
||||||
|
Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
|
||||||
|
|
||||||
|
fieldOverlay.setStrokeWidth(1);
|
||||||
|
fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY);
|
||||||
|
|
||||||
|
DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath());
|
||||||
|
} else if (currentSegment instanceof TurnSegment) {
|
||||||
|
Pose2d pose = currentSegment.getStartPose();
|
||||||
|
|
||||||
|
fieldOverlay.setFill(COLOR_ACTIVE_TURN);
|
||||||
|
fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3);
|
||||||
|
} else if (currentSegment instanceof WaitSegment) {
|
||||||
|
Pose2d pose = currentSegment.getStartPose();
|
||||||
|
|
||||||
|
fieldOverlay.setStrokeWidth(1);
|
||||||
|
fieldOverlay.setStroke(COLOR_ACTIVE_WAIT);
|
||||||
|
fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (targetPose != null) {
|
||||||
|
fieldOverlay.setStrokeWidth(1);
|
||||||
|
fieldOverlay.setStroke("#4CAF50");
|
||||||
|
DashboardUtil.drawRobot(fieldOverlay, targetPose);
|
||||||
|
}
|
||||||
|
|
||||||
|
fieldOverlay.setStroke("#3F51B5");
|
||||||
|
DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory);
|
||||||
|
|
||||||
|
fieldOverlay.setStroke("#3F51B5");
|
||||||
|
DashboardUtil.drawRobot(fieldOverlay, poseEstimate);
|
||||||
|
}
|
||||||
|
|
||||||
|
public Pose2d getLastPoseError() {
|
||||||
|
return lastPoseError;
|
||||||
|
}
|
||||||
|
|
||||||
|
public boolean isBusy() {
|
||||||
|
return currentTrajectorySequence != null;
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,40 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
|
||||||
|
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
public abstract class SequenceSegment {
|
||||||
|
private final double duration;
|
||||||
|
private final Pose2d startPose;
|
||||||
|
private final Pose2d endPose;
|
||||||
|
private final List<TrajectoryMarker> markers;
|
||||||
|
|
||||||
|
protected SequenceSegment(
|
||||||
|
double duration,
|
||||||
|
Pose2d startPose, Pose2d endPose,
|
||||||
|
List<TrajectoryMarker> markers
|
||||||
|
) {
|
||||||
|
this.duration = duration;
|
||||||
|
this.startPose = startPose;
|
||||||
|
this.endPose = endPose;
|
||||||
|
this.markers = markers;
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getDuration() {
|
||||||
|
return this.duration;
|
||||||
|
}
|
||||||
|
|
||||||
|
public Pose2d getStartPose() {
|
||||||
|
return startPose;
|
||||||
|
}
|
||||||
|
|
||||||
|
public Pose2d getEndPose() {
|
||||||
|
return endPose;
|
||||||
|
}
|
||||||
|
|
||||||
|
public List<TrajectoryMarker> getMarkers() {
|
||||||
|
return markers;
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,20 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
|
||||||
|
import java.util.Collections;
|
||||||
|
|
||||||
|
public final class TrajectorySegment extends SequenceSegment {
|
||||||
|
private final Trajectory trajectory;
|
||||||
|
|
||||||
|
public TrajectorySegment(Trajectory trajectory) {
|
||||||
|
// Note: Markers are already stored in the `Trajectory` itself.
|
||||||
|
// This class should not hold any markers
|
||||||
|
super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList());
|
||||||
|
this.trajectory = trajectory;
|
||||||
|
}
|
||||||
|
|
||||||
|
public Trajectory getTrajectory() {
|
||||||
|
return this.trajectory;
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,36 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.profile.MotionProfile;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
|
||||||
|
import com.acmerobotics.roadrunner.util.Angle;
|
||||||
|
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
public final class TurnSegment extends SequenceSegment {
|
||||||
|
private final double totalRotation;
|
||||||
|
private final MotionProfile motionProfile;
|
||||||
|
|
||||||
|
public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List<TrajectoryMarker> markers) {
|
||||||
|
super(
|
||||||
|
motionProfile.duration(),
|
||||||
|
startPose,
|
||||||
|
new Pose2d(
|
||||||
|
startPose.getX(), startPose.getY(),
|
||||||
|
Angle.norm(startPose.getHeading() + totalRotation)
|
||||||
|
),
|
||||||
|
markers
|
||||||
|
);
|
||||||
|
|
||||||
|
this.totalRotation = totalRotation;
|
||||||
|
this.motionProfile = motionProfile;
|
||||||
|
}
|
||||||
|
|
||||||
|
public final double getTotalRotation() {
|
||||||
|
return this.totalRotation;
|
||||||
|
}
|
||||||
|
|
||||||
|
public final MotionProfile getMotionProfile() {
|
||||||
|
return this.motionProfile;
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,12 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
|
||||||
|
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
public final class WaitSegment extends SequenceSegment {
|
||||||
|
public WaitSegment(Pose2d pose, double seconds, List<TrajectoryMarker> markers) {
|
||||||
|
super(seconds, pose, pose, markers);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,70 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.util;
|
||||||
|
|
||||||
|
import androidx.annotation.Nullable;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
|
||||||
|
import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
|
||||||
|
|
||||||
|
import java.io.IOException;
|
||||||
|
import java.io.InputStream;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set of utilities for loading trajectories from assets (the plugin save location).
|
||||||
|
*/
|
||||||
|
public class AssetsTrajectoryManager {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Loads the group config.
|
||||||
|
*/
|
||||||
|
public static @Nullable
|
||||||
|
TrajectoryGroupConfig loadGroupConfig() {
|
||||||
|
try {
|
||||||
|
InputStream inputStream = AppUtil.getDefContext().getAssets().open(
|
||||||
|
"trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
|
||||||
|
return TrajectoryConfigManager.loadGroupConfig(inputStream);
|
||||||
|
} catch (IOException e) {
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Loads a trajectory config with the given name.
|
||||||
|
*/
|
||||||
|
public static @Nullable TrajectoryConfig loadConfig(String name) {
|
||||||
|
try {
|
||||||
|
InputStream inputStream = AppUtil.getDefContext().getAssets().open(
|
||||||
|
"trajectory/" + name + ".yaml");
|
||||||
|
return TrajectoryConfigManager.loadConfig(inputStream);
|
||||||
|
} catch (IOException e) {
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Loads a trajectory builder with the given name.
|
||||||
|
*/
|
||||||
|
public static @Nullable TrajectoryBuilder loadBuilder(String name) {
|
||||||
|
TrajectoryGroupConfig groupConfig = loadGroupConfig();
|
||||||
|
TrajectoryConfig config = loadConfig(name);
|
||||||
|
if (groupConfig == null || config == null) {
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
return config.toTrajectoryBuilder(groupConfig);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Loads a trajectory with the given name.
|
||||||
|
*/
|
||||||
|
public static @Nullable Trajectory load(String name) {
|
||||||
|
TrajectoryBuilder builder = loadBuilder(name);
|
||||||
|
if (builder == null) {
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
return builder.build();
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,21 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.util;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* IMU axes signs in the order XYZ (after remapping).
|
||||||
|
*/
|
||||||
|
public enum AxesSigns {
|
||||||
|
PPP(0b000),
|
||||||
|
PPN(0b001),
|
||||||
|
PNP(0b010),
|
||||||
|
PNN(0b011),
|
||||||
|
NPP(0b100),
|
||||||
|
NPN(0b101),
|
||||||
|
NNP(0b110),
|
||||||
|
NNN(0b111);
|
||||||
|
|
||||||
|
public final int bVal;
|
||||||
|
|
||||||
|
AxesSigns(int bVal) {
|
||||||
|
this.bVal = bVal;
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,53 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.util;
|
||||||
|
|
||||||
|
import com.qualcomm.hardware.bosch.BNO055IMU;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Various utility functions for the BNO055 IMU.
|
||||||
|
*/
|
||||||
|
public class BNO055IMUUtil {
|
||||||
|
/**
|
||||||
|
* Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#ZYX}.
|
||||||
|
* Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
|
||||||
|
* mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
|
||||||
|
*
|
||||||
|
* Adapted from <a href="https://ftcforum.usfirst.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
|
||||||
|
*
|
||||||
|
* @param imu IMU
|
||||||
|
* @param order axes order
|
||||||
|
* @param signs axes signs
|
||||||
|
*/
|
||||||
|
public static void remapAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
|
||||||
|
try {
|
||||||
|
// the indices correspond with the 2-bit encodings specified in the datasheet
|
||||||
|
int[] indices = order.indices();
|
||||||
|
int axisMapConfig = 0;
|
||||||
|
axisMapConfig |= (indices[0] << 4);
|
||||||
|
axisMapConfig |= (indices[1] << 2);
|
||||||
|
axisMapConfig |= (indices[2] << 0);
|
||||||
|
|
||||||
|
// the BNO055 driver flips the first orientation vector so we also flip here
|
||||||
|
int axisMapSign = signs.bVal ^ (0b100 >> indices[0]);
|
||||||
|
|
||||||
|
// Enter CONFIG mode
|
||||||
|
imu.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
|
||||||
|
|
||||||
|
Thread.sleep(100);
|
||||||
|
|
||||||
|
// Write the AXIS_MAP_CONFIG register
|
||||||
|
imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
|
||||||
|
|
||||||
|
// Write the AXIS_MAP_SIGN register
|
||||||
|
imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSign & 0x07);
|
||||||
|
|
||||||
|
// Switch back to the previous mode
|
||||||
|
imu.write8(BNO055IMU.Register.OPR_MODE, imu.getParameters().mode.bVal & 0x0F);
|
||||||
|
|
||||||
|
Thread.sleep(100);
|
||||||
|
} catch (InterruptedException e) {
|
||||||
|
Thread.currentThread().interrupt();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,54 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.util;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.canvas.Canvas;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.geometry.Vector2d;
|
||||||
|
import com.acmerobotics.roadrunner.path.Path;
|
||||||
|
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
|
||||||
|
*/
|
||||||
|
public class DashboardUtil {
|
||||||
|
private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
|
||||||
|
private static final double ROBOT_RADIUS = 9; // in
|
||||||
|
|
||||||
|
|
||||||
|
public static void drawPoseHistory(Canvas canvas, List<Pose2d> poseHistory) {
|
||||||
|
double[] xPoints = new double[poseHistory.size()];
|
||||||
|
double[] yPoints = new double[poseHistory.size()];
|
||||||
|
for (int i = 0; i < poseHistory.size(); i++) {
|
||||||
|
Pose2d pose = poseHistory.get(i);
|
||||||
|
xPoints[i] = pose.getX();
|
||||||
|
yPoints[i] = pose.getY();
|
||||||
|
}
|
||||||
|
canvas.strokePolyline(xPoints, yPoints);
|
||||||
|
}
|
||||||
|
|
||||||
|
public static void drawSampledPath(Canvas canvas, Path path, double resolution) {
|
||||||
|
int samples = (int) Math.ceil(path.length() / resolution);
|
||||||
|
double[] xPoints = new double[samples];
|
||||||
|
double[] yPoints = new double[samples];
|
||||||
|
double dx = path.length() / (samples - 1);
|
||||||
|
for (int i = 0; i < samples; i++) {
|
||||||
|
double displacement = i * dx;
|
||||||
|
Pose2d pose = path.get(displacement);
|
||||||
|
xPoints[i] = pose.getX();
|
||||||
|
yPoints[i] = pose.getY();
|
||||||
|
}
|
||||||
|
canvas.strokePolyline(xPoints, yPoints);
|
||||||
|
}
|
||||||
|
|
||||||
|
public static void drawSampledPath(Canvas canvas, Path path) {
|
||||||
|
drawSampledPath(canvas, path, DEFAULT_RESOLUTION);
|
||||||
|
}
|
||||||
|
|
||||||
|
public static void drawRobot(Canvas canvas, Pose2d pose) {
|
||||||
|
canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS);
|
||||||
|
Vector2d v = pose.headingVec().times(ROBOT_RADIUS);
|
||||||
|
double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2;
|
||||||
|
double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY();
|
||||||
|
canvas.strokeLine(x1, y1, x2, y2);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,98 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.util;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.util.NanoClock;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
|
||||||
|
* slot's motor direction
|
||||||
|
*/
|
||||||
|
public class Encoder {
|
||||||
|
private final static int CPS_STEP = 0x10000;
|
||||||
|
|
||||||
|
private static double inverseOverflow(double input, double estimate) {
|
||||||
|
double real = input;
|
||||||
|
while (Math.abs(estimate - real) > CPS_STEP / 2.0) {
|
||||||
|
real += Math.signum(estimate - real) * CPS_STEP;
|
||||||
|
}
|
||||||
|
return real;
|
||||||
|
}
|
||||||
|
|
||||||
|
public enum Direction {
|
||||||
|
FORWARD(1),
|
||||||
|
REVERSE(-1);
|
||||||
|
|
||||||
|
private int multiplier;
|
||||||
|
|
||||||
|
Direction(int multiplier) {
|
||||||
|
this.multiplier = multiplier;
|
||||||
|
}
|
||||||
|
|
||||||
|
public int getMultiplier() {
|
||||||
|
return multiplier;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private DcMotorEx motor;
|
||||||
|
private NanoClock clock;
|
||||||
|
|
||||||
|
private Direction direction;
|
||||||
|
|
||||||
|
private int lastPosition;
|
||||||
|
private double velocityEstimate;
|
||||||
|
private double lastUpdateTime;
|
||||||
|
|
||||||
|
public Encoder(DcMotorEx motor, NanoClock clock) {
|
||||||
|
this.motor = motor;
|
||||||
|
this.clock = clock;
|
||||||
|
|
||||||
|
this.direction = Direction.FORWARD;
|
||||||
|
|
||||||
|
this.lastPosition = 0;
|
||||||
|
this.velocityEstimate = 0.0;
|
||||||
|
this.lastUpdateTime = clock.seconds();
|
||||||
|
}
|
||||||
|
|
||||||
|
public Encoder(DcMotorEx motor) {
|
||||||
|
this(motor, NanoClock.system());
|
||||||
|
}
|
||||||
|
|
||||||
|
public Direction getDirection() {
|
||||||
|
return direction;
|
||||||
|
}
|
||||||
|
|
||||||
|
private int getMultiplier() {
|
||||||
|
return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Allows you to set the direction of the counts and velocity without modifying the motor's direction state
|
||||||
|
* @param direction either reverse or forward depending on if encoder counts should be negated
|
||||||
|
*/
|
||||||
|
public void setDirection(Direction direction) {
|
||||||
|
this.direction = direction;
|
||||||
|
}
|
||||||
|
|
||||||
|
public int getCurrentPosition() {
|
||||||
|
int multiplier = getMultiplier();
|
||||||
|
int currentPosition = motor.getCurrentPosition() * multiplier;
|
||||||
|
if (currentPosition != lastPosition) {
|
||||||
|
double currentTime = clock.seconds();
|
||||||
|
double dt = currentTime - lastUpdateTime;
|
||||||
|
velocityEstimate = (currentPosition - lastPosition) / dt;
|
||||||
|
lastPosition = currentPosition;
|
||||||
|
lastUpdateTime = currentTime;
|
||||||
|
}
|
||||||
|
return currentPosition;
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getRawVelocity() {
|
||||||
|
int multiplier = getMultiplier();
|
||||||
|
return motor.getVelocity() * multiplier;
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getCorrectedVelocity() {
|
||||||
|
return inverseOverflow(getRawVelocity(), velocityEstimate);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,60 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.util;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
|
||||||
|
|
||||||
|
import java.io.File;
|
||||||
|
import java.util.ArrayList;
|
||||||
|
import java.util.Collections;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Utility functions for log files.
|
||||||
|
*/
|
||||||
|
public class LoggingUtil {
|
||||||
|
public static final File ROAD_RUNNER_FOLDER =
|
||||||
|
new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
|
||||||
|
|
||||||
|
private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
|
||||||
|
|
||||||
|
private static void buildLogList(List<File> logFiles, File dir) {
|
||||||
|
for (File file : dir.listFiles()) {
|
||||||
|
if (file.isDirectory()) {
|
||||||
|
buildLogList(logFiles, file);
|
||||||
|
} else {
|
||||||
|
logFiles.add(file);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void pruneLogsIfNecessary() {
|
||||||
|
List<File> logFiles = new ArrayList<>();
|
||||||
|
buildLogList(logFiles, ROAD_RUNNER_FOLDER);
|
||||||
|
Collections.sort(logFiles, (lhs, rhs) ->
|
||||||
|
Long.compare(lhs.lastModified(), rhs.lastModified()));
|
||||||
|
|
||||||
|
long dirSize = 0;
|
||||||
|
for (File file: logFiles) {
|
||||||
|
dirSize += file.length();
|
||||||
|
}
|
||||||
|
|
||||||
|
while (dirSize > LOG_QUOTA) {
|
||||||
|
if (logFiles.size() == 0) break;
|
||||||
|
File fileToRemove = logFiles.remove(0);
|
||||||
|
dirSize -= fileToRemove.length();
|
||||||
|
//noinspection ResultOfMethodCallIgnored
|
||||||
|
fileToRemove.delete();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Obtain a log file with the provided name
|
||||||
|
*/
|
||||||
|
public static File getLogFile(String name) {
|
||||||
|
//noinspection ResultOfMethodCallIgnored
|
||||||
|
ROAD_RUNNER_FOLDER.mkdirs();
|
||||||
|
|
||||||
|
pruneLogsIfNecessary();
|
||||||
|
|
||||||
|
return new File(ROAD_RUNNER_FOLDER, name);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,124 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.util;
|
||||||
|
|
||||||
|
import com.qualcomm.hardware.lynx.LynxModule;
|
||||||
|
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.internal.system.Misc;
|
||||||
|
|
||||||
|
import java.util.HashMap;
|
||||||
|
import java.util.Map;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Collection of utilites for interacting with Lynx modules.
|
||||||
|
*/
|
||||||
|
public class LynxModuleUtil {
|
||||||
|
|
||||||
|
private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Parsed representation of a Lynx module firmware version.
|
||||||
|
*/
|
||||||
|
public static class LynxFirmwareVersion implements Comparable<LynxFirmwareVersion> {
|
||||||
|
public final int major;
|
||||||
|
public final int minor;
|
||||||
|
public final int eng;
|
||||||
|
|
||||||
|
public LynxFirmwareVersion(int major, int minor, int eng) {
|
||||||
|
this.major = major;
|
||||||
|
this.minor = minor;
|
||||||
|
this.eng = eng;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean equals(Object other) {
|
||||||
|
if (other instanceof LynxFirmwareVersion) {
|
||||||
|
LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
|
||||||
|
return major == otherVersion.major && minor == otherVersion.minor &&
|
||||||
|
eng == otherVersion.eng;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public int compareTo(LynxFirmwareVersion other) {
|
||||||
|
int majorComp = Integer.compare(major, other.major);
|
||||||
|
if (majorComp == 0) {
|
||||||
|
int minorComp = Integer.compare(minor, other.minor);
|
||||||
|
if (minorComp == 0) {
|
||||||
|
return Integer.compare(eng, other.eng);
|
||||||
|
} else {
|
||||||
|
return minorComp;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
return majorComp;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public String toString() {
|
||||||
|
return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Retrieve and parse Lynx module firmware version.
|
||||||
|
* @param module Lynx module
|
||||||
|
* @return parsed firmware version
|
||||||
|
*/
|
||||||
|
public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) {
|
||||||
|
String versionString = module.getNullableFirmwareVersionString();
|
||||||
|
if (versionString == null) {
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
|
||||||
|
String[] parts = versionString.split("[ :,]+");
|
||||||
|
try {
|
||||||
|
// note: for now, we ignore the hardware entry
|
||||||
|
return new LynxFirmwareVersion(
|
||||||
|
Integer.parseInt(parts[3]),
|
||||||
|
Integer.parseInt(parts[5]),
|
||||||
|
Integer.parseInt(parts[7])
|
||||||
|
);
|
||||||
|
} catch (NumberFormatException e) {
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Exception indicating an outdated Lynx firmware version.
|
||||||
|
*/
|
||||||
|
public static class LynxFirmwareVersionException extends RuntimeException {
|
||||||
|
public LynxFirmwareVersionException(String detailMessage) {
|
||||||
|
super(detailMessage);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
|
||||||
|
* @param hardwareMap hardware map containing Lynx modules
|
||||||
|
*/
|
||||||
|
public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {
|
||||||
|
Map<String, LynxFirmwareVersion> outdatedModules = new HashMap<>();
|
||||||
|
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
|
||||||
|
LynxFirmwareVersion version = getFirmwareVersion(module);
|
||||||
|
if (version == null || version.compareTo(MIN_VERSION) < 0) {
|
||||||
|
for (String name : hardwareMap.getNamesOf(module)) {
|
||||||
|
outdatedModules.put(name, version);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (outdatedModules.size() > 0) {
|
||||||
|
StringBuilder msgBuilder = new StringBuilder();
|
||||||
|
msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
|
||||||
|
msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
|
||||||
|
MIN_VERSION.toString()));
|
||||||
|
for (Map.Entry<String, LynxFirmwareVersion> entry : outdatedModules.entrySet()) {
|
||||||
|
msgBuilder.append(Misc.formatInvariant(
|
||||||
|
"\t%s: %s\n", entry.getKey(),
|
||||||
|
entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
|
||||||
|
}
|
||||||
|
throw new LynxFirmwareVersionException(msgBuilder.toString());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,156 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.util;
|
||||||
|
|
||||||
|
import androidx.annotation.Nullable;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.kinematics.Kinematics;
|
||||||
|
|
||||||
|
import org.apache.commons.math3.stat.regression.SimpleRegression;
|
||||||
|
|
||||||
|
import java.io.File;
|
||||||
|
import java.io.FileNotFoundException;
|
||||||
|
import java.io.PrintWriter;
|
||||||
|
import java.util.ArrayList;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Various regression utilities.
|
||||||
|
*/
|
||||||
|
public class RegressionUtil {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Feedforward parameter estimates from the ramp regression and additional summary statistics
|
||||||
|
*/
|
||||||
|
public static class RampResult {
|
||||||
|
public final double kV, kStatic, rSquare;
|
||||||
|
|
||||||
|
public RampResult(double kV, double kStatic, double rSquare) {
|
||||||
|
this.kV = kV;
|
||||||
|
this.kStatic = kStatic;
|
||||||
|
this.rSquare = rSquare;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Feedforward parameter estimates from the ramp regression and additional summary statistics
|
||||||
|
*/
|
||||||
|
public static class AccelResult {
|
||||||
|
public final double kA, rSquare;
|
||||||
|
|
||||||
|
public AccelResult(double kA, double rSquare) {
|
||||||
|
this.kA = kA;
|
||||||
|
this.rSquare = rSquare;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Numerically compute dy/dx from the given x and y values. The returned list is padded to match
|
||||||
|
* the length of the original sequences.
|
||||||
|
*
|
||||||
|
* @param x x-values
|
||||||
|
* @param y y-values
|
||||||
|
* @return derivative values
|
||||||
|
*/
|
||||||
|
private static List<Double> numericalDerivative(List<Double> x, List<Double> y) {
|
||||||
|
List<Double> deriv = new ArrayList<>(x.size());
|
||||||
|
for (int i = 1; i < x.size() - 1; i++) {
|
||||||
|
deriv.add(
|
||||||
|
(y.get(i + 1) - y.get(i - 1)) /
|
||||||
|
(x.get(i + 1) - x.get(i - 1))
|
||||||
|
);
|
||||||
|
}
|
||||||
|
// copy endpoints to pad output
|
||||||
|
deriv.add(0, deriv.get(0));
|
||||||
|
deriv.add(deriv.get(deriv.size() - 1));
|
||||||
|
return deriv;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Run regression to compute velocity and static feedforward from ramp test data.
|
||||||
|
*
|
||||||
|
* Here's the general procedure for gathering the requisite data:
|
||||||
|
* 1. Slowly ramp the motor power/voltage and record encoder values along the way.
|
||||||
|
* 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope
|
||||||
|
* (kV) and an optional intercept (kStatic).
|
||||||
|
*
|
||||||
|
* @param timeSamples time samples
|
||||||
|
* @param positionSamples position samples
|
||||||
|
* @param powerSamples power samples
|
||||||
|
* @param fitStatic fit kStatic
|
||||||
|
* @param file log file
|
||||||
|
*/
|
||||||
|
public static RampResult fitRampData(List<Double> timeSamples, List<Double> positionSamples,
|
||||||
|
List<Double> powerSamples, boolean fitStatic,
|
||||||
|
@Nullable File file) {
|
||||||
|
if (file != null) {
|
||||||
|
try (PrintWriter pw = new PrintWriter(file)) {
|
||||||
|
pw.println("time,position,power");
|
||||||
|
for (int i = 0; i < timeSamples.size(); i++) {
|
||||||
|
double time = timeSamples.get(i);
|
||||||
|
double pos = positionSamples.get(i);
|
||||||
|
double power = powerSamples.get(i);
|
||||||
|
pw.println(time + "," + pos + "," + power);
|
||||||
|
}
|
||||||
|
} catch (FileNotFoundException e) {
|
||||||
|
// ignore
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
|
||||||
|
|
||||||
|
SimpleRegression rampReg = new SimpleRegression(fitStatic);
|
||||||
|
for (int i = 0; i < timeSamples.size(); i++) {
|
||||||
|
double vel = velSamples.get(i);
|
||||||
|
double power = powerSamples.get(i);
|
||||||
|
|
||||||
|
rampReg.addData(vel, power);
|
||||||
|
}
|
||||||
|
|
||||||
|
return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()),
|
||||||
|
rampReg.getRSquare());
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Run regression to compute acceleration feedforward.
|
||||||
|
*
|
||||||
|
* @param timeSamples time samples
|
||||||
|
* @param positionSamples position samples
|
||||||
|
* @param powerSamples power samples
|
||||||
|
* @param rampResult ramp result
|
||||||
|
* @param file log file
|
||||||
|
*/
|
||||||
|
public static AccelResult fitAccelData(List<Double> timeSamples, List<Double> positionSamples,
|
||||||
|
List<Double> powerSamples, RampResult rampResult,
|
||||||
|
@Nullable File file) {
|
||||||
|
if (file != null) {
|
||||||
|
try (PrintWriter pw = new PrintWriter(file)) {
|
||||||
|
pw.println("time,position,power");
|
||||||
|
for (int i = 0; i < timeSamples.size(); i++) {
|
||||||
|
double time = timeSamples.get(i);
|
||||||
|
double pos = positionSamples.get(i);
|
||||||
|
double power = powerSamples.get(i);
|
||||||
|
pw.println(time + "," + pos + "," + power);
|
||||||
|
}
|
||||||
|
} catch (FileNotFoundException e) {
|
||||||
|
// ignore
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
|
||||||
|
List<Double> accelSamples = numericalDerivative(timeSamples, velSamples);
|
||||||
|
|
||||||
|
SimpleRegression accelReg = new SimpleRegression(false);
|
||||||
|
for (int i = 0; i < timeSamples.size(); i++) {
|
||||||
|
double vel = velSamples.get(i);
|
||||||
|
double accel = accelSamples.get(i);
|
||||||
|
double power = powerSamples.get(i);
|
||||||
|
|
||||||
|
double powerFromVel = Kinematics.calculateMotorFeedforward(
|
||||||
|
vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic);
|
||||||
|
double powerFromAccel = power - powerFromVel;
|
||||||
|
|
||||||
|
accelReg.addData(accel, powerFromAccel);
|
||||||
|
}
|
||||||
|
|
||||||
|
return new AccelResult(Math.abs(accelReg.getSlope()), accelReg.getRSquare());
|
||||||
|
}
|
||||||
|
}
|
|
@ -5,6 +5,7 @@ repositories {
|
||||||
flatDir {
|
flatDir {
|
||||||
dirs rootProject.file('libs')
|
dirs rootProject.file('libs')
|
||||||
}
|
}
|
||||||
|
maven { url = 'https://maven.brott.dev/' }
|
||||||
}
|
}
|
||||||
|
|
||||||
dependencies {
|
dependencies {
|
||||||
|
@ -19,5 +20,6 @@ dependencies {
|
||||||
implementation 'org.tensorflow:tensorflow-lite-task-vision:0.2.0'
|
implementation 'org.tensorflow:tensorflow-lite-task-vision:0.2.0'
|
||||||
implementation 'androidx.appcompat:appcompat:1.2.0'
|
implementation 'androidx.appcompat:appcompat:1.2.0'
|
||||||
implementation 'org.firstinspires.ftc:gameAssets-FreightFrenzy:1.0.0'
|
implementation 'org.firstinspires.ftc:gameAssets-FreightFrenzy:1.0.0'
|
||||||
|
implementation 'com.acmerobotics.dashboard:dashboard:0.4.3'
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in a new issue