Merge remote-tracking branch 'origin/branch-silver-14493' into branch-silver-14493
This commit is contained in:
robotics1
107
TeamCode/src/main/java/GeneratedPath.java
Normal file
107
TeamCode/src/main/java/GeneratedPath.java
Normal file
@ -0,0 +1,107 @@
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
public class GeneratedPath {
|
||||
public GeneratedPath() {
|
||||
PathBuilder builder = new PathBuilder();
|
||||
|
||||
builder
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(9.757, 84.983, Point.CARTESIAN),
|
||||
new Point(28.573, 76.302, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(28.573, 76.302, Point.CARTESIAN),
|
||||
new Point(36.203, 76.140, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(36.203, 76.140, Point.CARTESIAN),
|
||||
new Point(35.067, 35.716, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(35.067, 35.716, Point.CARTESIAN),
|
||||
new Point(73.705, 34.742, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierLine(
|
||||
new Point(73.705, 34.742, Point.CARTESIAN),
|
||||
new Point(73.705, 24.839, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 6
|
||||
new BezierLine(
|
||||
new Point(73.705, 24.839, Point.CARTESIAN),
|
||||
new Point(7.630, 26.462, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 7
|
||||
new BezierLine(
|
||||
new Point(7.630, 26.462, Point.CARTESIAN),
|
||||
new Point(64.126, 22.728, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 8
|
||||
new BezierLine(
|
||||
new Point(64.126, 22.728, Point.CARTESIAN),
|
||||
new Point(63.964, 13.150, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 9
|
||||
new BezierLine(
|
||||
new Point(63.964, 13.150, Point.CARTESIAN),
|
||||
new Point(12.338, 15.260, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 10
|
||||
new BezierLine(
|
||||
new Point(12.338, 15.260, Point.CARTESIAN),
|
||||
new Point(63.802, 13.150, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 11
|
||||
new BezierLine(
|
||||
new Point(63.802, 13.150, Point.CARTESIAN),
|
||||
new Point(63.639, 11.689, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 12
|
||||
new BezierLine(
|
||||
new Point(63.639, 11.689, Point.CARTESIAN),
|
||||
new Point(12.014, 11.689, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation();
|
||||
}
|
||||
}
|
||||
236
TeamCode/src/main/java/bBlueAutoV1.java
Normal file
236
TeamCode/src/main/java/bBlueAutoV1.java
Normal file
@ -0,0 +1,236 @@
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
@Autonomous(name = "bBlueAutoV1", group = "V1")
|
||||
public class bBlueAutoV1 extends OpMode {
|
||||
|
||||
public Telemetry telemetry;
|
||||
public Follower robot;
|
||||
public PathChain path;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
robot = new Follower(hardwareMap);
|
||||
|
||||
PathBuilder builder = new PathBuilder();
|
||||
|
||||
path = builder
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(9.757, 84.983, Point.CARTESIAN),
|
||||
new Point(28.573, 76.302, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(28.573, 76.302, Point.CARTESIAN),
|
||||
new Point(36.203, 76.140, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(36.203, 76.140, Point.CARTESIAN),
|
||||
new Point(35.067, 35.716, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(35.067, 35.716, Point.CARTESIAN),
|
||||
new Point(73.705, 34.742, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierLine(
|
||||
new Point(73.705, 34.742, Point.CARTESIAN),
|
||||
new Point(73.705, 24.839, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 6
|
||||
new BezierLine(
|
||||
new Point(73.705, 24.839, Point.CARTESIAN),
|
||||
new Point(7.630, 26.462, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 7
|
||||
new BezierLine(
|
||||
new Point(7.630, 26.462, Point.CARTESIAN),
|
||||
new Point(64.126, 22.728, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 8
|
||||
new BezierLine(
|
||||
new Point(64.126, 22.728, Point.CARTESIAN),
|
||||
new Point(63.964, 13.150, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 9
|
||||
new BezierLine(
|
||||
new Point(63.964, 13.150, Point.CARTESIAN),
|
||||
new Point(12.338, 15.260, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 10
|
||||
new BezierLine(
|
||||
new Point(12.338, 15.260, Point.CARTESIAN),
|
||||
new Point(63.802, 13.150, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 11
|
||||
new BezierLine(
|
||||
new Point(63.802, 13.150, Point.CARTESIAN),
|
||||
new Point(63.639, 11.689, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 12
|
||||
new BezierLine(
|
||||
new Point(63.639, 11.689, Point.CARTESIAN),
|
||||
new Point(12.014, 11.689, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 13
|
||||
new BezierLine(
|
||||
new Point(12.014, 11.689, Point.CARTESIAN),
|
||||
new Point(62.665, 30.196, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 14
|
||||
new BezierLine(
|
||||
new Point(62.665, 30.196, Point.CARTESIAN),
|
||||
new Point(13.312, 51.463, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 15
|
||||
new BezierLine(
|
||||
new Point(13.312, 51.463, Point.CARTESIAN),
|
||||
new Point(16.234, 103.738, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 16
|
||||
new BezierLine(
|
||||
new Point(16.234, 103.738, Point.CARTESIAN),
|
||||
new Point(68.023, 108.284, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 17
|
||||
new BezierLine(
|
||||
new Point(68.023, 108.284, Point.CARTESIAN),
|
||||
new Point(68.185, 121.109, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 18
|
||||
new BezierLine(
|
||||
new Point(68.185, 121.109, Point.CARTESIAN),
|
||||
new Point(21.754, 119.811, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 19
|
||||
new BezierLine(
|
||||
new Point(21.754, 119.811, Point.CARTESIAN),
|
||||
new Point(11.526, 129.227, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 20
|
||||
new BezierLine(
|
||||
new Point(11.526, 129.227, Point.CARTESIAN),
|
||||
new Point(72.568, 111.856, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 21
|
||||
new BezierLine(
|
||||
new Point(72.568, 111.856, Point.CARTESIAN),
|
||||
new Point(58.607, 128.902, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 22
|
||||
new BezierLine(
|
||||
new Point(58.607, 128.902, Point.CARTESIAN),
|
||||
new Point(11.364, 130.850, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 23
|
||||
new BezierLine(
|
||||
new Point(11.364, 130.850, Point.CARTESIAN),
|
||||
new Point(58.931, 128.577, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 24
|
||||
new BezierLine(
|
||||
new Point(58.931, 128.577, Point.CARTESIAN),
|
||||
new Point(58.769, 133.123, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.addPath(
|
||||
// Line 25
|
||||
new BezierLine(
|
||||
new Point(58.769, 133.123, Point.CARTESIAN),
|
||||
new Point(13.475, 133.935, Point.CARTESIAN)
|
||||
)
|
||||
).build();
|
||||
;
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
robot.update();
|
||||
if (robot.atParametricEnd())
|
||||
robot.followPath(path);
|
||||
robot.telemetryDebug(telemetry);
|
||||
}
|
||||
}
|
||||
8
TeamCode/src/main/java/nbRedAutoV1/Java.java
Normal file
8
TeamCode/src/main/java/nbRedAutoV1/Java.java
Normal file
@ -0,0 +1,8 @@
|
||||
package nbRedAutoV1;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
public class Java {
|
||||
}
|
||||
174
TeamCode/src/main/java/nbRedAutoV1/bRedAutoV1.java
Normal file
174
TeamCode/src/main/java/nbRedAutoV1/bRedAutoV1.java
Normal file
@ -0,0 +1,174 @@
|
||||
package nbRedAutoV1;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
public class bRedAutoV1 {
|
||||
|
||||
public bRedAutoV1() {
|
||||
PathBuilder builder = new PathBuilder();
|
||||
|
||||
builder
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(133.935, 83.770, Point.CARTESIAN),
|
||||
new Point(79.874, 117.213, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(79.874, 117.213, Point.CARTESIAN),
|
||||
new Point(79.874, 120.785, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(79.874, 120.785, Point.CARTESIAN),
|
||||
new Point(131.824, 118.349, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(131.824, 118.349, Point.CARTESIAN),
|
||||
new Point(79.549, 120.460, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierLine(
|
||||
new Point(79.549, 120.460, Point.CARTESIAN),
|
||||
new Point(79.549, 128.740, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 6
|
||||
new BezierLine(
|
||||
new Point(79.549, 128.740, Point.CARTESIAN),
|
||||
new Point(131.337, 128.090, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 7
|
||||
new BezierLine(
|
||||
new Point(131.337, 128.090, Point.CARTESIAN),
|
||||
new Point(79.549, 128.740, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 8
|
||||
new BezierLine(
|
||||
new Point(79.549, 128.740, Point.CARTESIAN),
|
||||
new Point(79.549, 133.610, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 9
|
||||
new BezierLine(
|
||||
new Point(79.549, 133.610, Point.CARTESIAN),
|
||||
new Point(130.850, 133.285, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 10
|
||||
new BezierLine(
|
||||
new Point(130.850, 133.285, Point.CARTESIAN),
|
||||
new Point(130.201, 36.528, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 11
|
||||
new BezierLine(
|
||||
new Point(130.201, 36.528, Point.CARTESIAN),
|
||||
new Point(84.095, 36.203, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 12
|
||||
new BezierLine(
|
||||
new Point(84.095, 36.203, Point.CARTESIAN),
|
||||
new Point(84.095, 23.378, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 13
|
||||
new BezierLine(
|
||||
new Point(84.095, 23.378, Point.CARTESIAN),
|
||||
new Point(119.811, 23.378, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 14
|
||||
new BezierLine(
|
||||
new Point(119.811, 23.378, Point.CARTESIAN),
|
||||
new Point(127.603, 13.475, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 15
|
||||
new BezierLine(
|
||||
new Point(127.603, 13.475, Point.CARTESIAN),
|
||||
new Point(88.640, 28.248, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 16
|
||||
new BezierLine(
|
||||
new Point(88.640, 28.248, Point.CARTESIAN),
|
||||
new Point(87.666, 15.910, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 17
|
||||
new BezierLine(
|
||||
new Point(87.666, 15.910, Point.CARTESIAN),
|
||||
new Point(127.603, 12.014, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 18
|
||||
new BezierLine(
|
||||
new Point(127.603, 12.014, Point.CARTESIAN),
|
||||
new Point(86.692, 12.825, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 19
|
||||
new BezierLine(
|
||||
new Point(86.692, 12.825, Point.CARTESIAN),
|
||||
new Point(86.692, 10.390, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 20
|
||||
new BezierLine(
|
||||
new Point(86.692, 10.390, Point.CARTESIAN),
|
||||
new Point(126.467, 9.903, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation();
|
||||
}
|
||||
}
|
||||
@ -56,19 +56,19 @@ public class PedroConstants {
|
||||
public static final double ROBOT_WEIGHT_IN_KG = 10.5;
|
||||
|
||||
// Maximum velocity of the robot going forward
|
||||
public static final double ROBOT_SPEED_FORWARD = 72.0693;
|
||||
public static final double ROBOT_SPEED_FORWARD = 79.0257;
|
||||
|
||||
// Maximum velocity of the robot going right
|
||||
public static final double ROBOT_SPEED_LATERAL = 24.1401;
|
||||
public static final double ROBOT_SPEED_LATERAL = 12.3941;
|
||||
|
||||
// Rate of deceleration when power is cut-off when the robot is moving forward
|
||||
public static final double FORWARD_ZERO_POWER_ACCEL = -74.3779;
|
||||
public static final double FORWARD_ZERO_POWER_ACCEL = -50.7945;
|
||||
|
||||
// Rate of deceleration when power is cut-off when the robot is moving to the right
|
||||
public static final double LATERAL_ZERO_POWER_ACCEL = -111.8409;
|
||||
public static final double LATERAL_ZERO_POWER_ACCEL = -92.733;
|
||||
|
||||
// Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
|
||||
public static final double ZERO_POWER_ACCEL_MULT = 4;
|
||||
public static final double ZERO_POWER_ACCEL_MULT = 2.5;
|
||||
|
||||
/* Centripetal force correction - increase if robot is correcting into the path
|
||||
- decrease if robot is correcting away from the path */
|
||||
|
||||
@ -71,7 +71,8 @@ measurements will be in centimeters.
|
||||
of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but
|
||||
what works best for you is most important. Higher numbers will cause a faster brake, but increase
|
||||
oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more sensitive than the others. For reference,
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more
|
||||
* sensitive than the others. For reference,
|
||||
my P values were in the hundredths and thousandths place values, and my D values were in the hundred
|
||||
thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and
|
||||
`useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`
|
||||
|
||||
Reference in New Issue
Block a user