use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Relic_Main by TeamOverdrive.
the class TextFileDeserializer method deserialize.
public Path deserialize(String serialized) {
StringTokenizer tokenizer = new StringTokenizer(serialized, "\n");
System.out.println("Parsing path string...");
System.out.println("String has " + serialized.length() + " chars");
System.out.println("Found " + tokenizer.countTokens() + " tokens");
String name = tokenizer.nextToken();
int num_elements = Integer.parseInt(tokenizer.nextToken());
Trajectory left = new Trajectory(num_elements);
for (int i = 0; i < num_elements; ++i) {
Trajectory.Segment segment = new Trajectory.Segment();
StringTokenizer line_tokenizer = new StringTokenizer(tokenizer.nextToken(), " ");
segment.pos = Double.parseDouble(line_tokenizer.nextToken());
segment.vel = Double.parseDouble(line_tokenizer.nextToken());
segment.acc = Double.parseDouble(line_tokenizer.nextToken());
segment.jerk = Double.parseDouble(line_tokenizer.nextToken());
segment.heading = Double.parseDouble(line_tokenizer.nextToken());
segment.dt = Double.parseDouble(line_tokenizer.nextToken());
segment.x = Double.parseDouble(line_tokenizer.nextToken());
segment.y = Double.parseDouble(line_tokenizer.nextToken());
left.setSegment(i, segment);
}
Trajectory right = new Trajectory(num_elements);
for (int i = 0; i < num_elements; ++i) {
Trajectory.Segment segment = new Trajectory.Segment();
StringTokenizer line_tokenizer = new StringTokenizer(tokenizer.nextToken(), " ");
segment.pos = Double.parseDouble(line_tokenizer.nextToken());
segment.vel = Double.parseDouble(line_tokenizer.nextToken());
segment.acc = Double.parseDouble(line_tokenizer.nextToken());
segment.jerk = Double.parseDouble(line_tokenizer.nextToken());
segment.heading = Double.parseDouble(line_tokenizer.nextToken());
segment.dt = Double.parseDouble(line_tokenizer.nextToken());
segment.x = Double.parseDouble(line_tokenizer.nextToken());
segment.y = Double.parseDouble(line_tokenizer.nextToken());
right.setSegment(i, segment);
}
System.out.println("...finished parsing path from string.");
return new Path(name, new Trajectory.Pair(left, right));
}
use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Relic_Main by TeamOverdrive.
the class Arc method calculate.
public static Path calculate(TrajectoryGenerator.Config config, TrajectoryGenerator.Strategy strategy, double start_velocity, double start_heading, double distance, double goal_velocity, double goal_heading) {
// Generate trajectory based to the inputs
Trajectory reference = TrajectoryGenerator.generate(config, strategy, // start velocity
start_velocity, // start heading
start_heading, // goal position
Math.abs(distance), // goal velocity
goal_velocity, // goal angle
goal_heading);
Trajectory leftProfile = reference;
Trajectory rightProfile = reference.copy();
// Calculate the radius of the arc
double radius = Math.copySign(Math.abs(Math.abs(distance) / (goal_heading - start_heading * Math.PI / 180.0)), distance);
double width = 12.625;
// Find the difference between the left and right motors
double faster = (radius + (width / 2.0)) / radius;
double slower = (radius - (width / 2.0)) / radius;
System.out.println(faster);
System.out.println(slower);
// Determine which way to curve
if (goal_heading - start_heading > 0) {
leftProfile.scale(faster);
rightProfile.scale(slower);
} else if (goal_heading - start_heading < 0) {
leftProfile.scale(slower);
rightProfile.scale(faster);
}
Path ref = new Path("Arc", new Trajectory.Pair(leftProfile, rightProfile));
if (distance < 0)
InvertY.calculate(ref);
return ref;
}
use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Relic_Main by TeamOverdrive.
the class TestPointTurning method main.
public static void main(String... args) {
TrajectoryGenerator.Config config = new TrajectoryGenerator.Config();
// In/s
config.max_vel = 2.5 * 12;
// In/s^2
config.max_acc = 8 * 12;
// In/s^3
config.max_jerk = 10 * 12;
// seconds, change of time in each update
config.dt = 0.01;
double angle = Math.PI / 2;
TrajectoryGenerator.Strategy strategy = TrajectoryGenerator.SCurvesStrategy;
Trajectory ref = TrajectoryGenerator.generate(config, strategy, 0, 0, 1e-10, 0, angle);
Trajectory leftProfile = ref;
Trajectory rightProfile = ref.copy();
// Calculate the radius of the arc
double radius = Math.abs(Math.abs(1e-10) / (angle * Math.PI / 180.0));
double width = 12.625;
// Find the difference between the left and right motors
double faster = (radius + (width / 2.0)) / radius;
double slower = (radius - (width / 2.0)) / radius;
// Determine which way to curve
if (angle > 0) {
leftProfile.scale(faster);
rightProfile.scale(slower);
} else {
leftProfile.scale(slower);
rightProfile.scale(faster);
}
System.out.println(leftProfile.toString());
System.out.println("====================");
System.out.println(rightProfile.toString());
System.out.println("Angle: " + angle);
System.out.println(angle - rightProfile.getSegment(rightProfile.getNumSegments() - 1).heading);
}
use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Relic_Main by TeamOverdrive.
the class TestingSplinesSingleTrajectory method runOpMode.
@Override
public void runOpMode() throws InterruptedException {
telemetry.addData("Started", "");
telemetry.update();
TrajectoryGenerator.Config config = new TrajectoryGenerator.Config();
// In/s
config.max_vel = 2.0 * 12;
// In/s^2
config.max_acc = 0.6 * 12;
// In/s^3
config.max_jerk = 0.4 * 12;
// seconds
config.dt = 0.01;
Trajectory trajectory = TrajectoryGenerator.generate(config, TrajectoryGenerator.SCurvesStrategy, 0, 0, 10, 0, 0);
telemetry.addData("Trajectory Generation Finished", "");
telemetry.update();
double gearing = Constants.DRIVE_GEAR_REDUCTION;
// Volts
double Vmax = 12;
// lbs
double mass = 48.0 / 9.8;
double numberOfMotors = 2;
// lb-in
double stallTorque = 1.531179;
double velocityMax = (5400 * Math.PI * Constants.WHEEL_DIAMETER_INCHES) / (gearing);
double kv = Vmax / velocityMax;
double accelerationMax = (2 * numberOfMotors * stallTorque * gearing) / (4.0 * mass);
double ka = Vmax / accelerationMax;
FollowerConfig followerConfig = new FollowerConfig(0.1, 0, kv, ka, Math.PI / 1000);
TrajectoryFollower follower = new TrajectoryFollower("Follower");
follower.configure(followerConfig.get()[0], followerConfig.get()[1], followerConfig.get()[2], followerConfig.get()[3], followerConfig.get()[4]);
follower.setTrajectory(trajectory);
Drive mDrive = new Drive();
mDrive.init(this, true);
mDrive.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
waitForStart();
ElapsedTime time = new ElapsedTime();
sleep(10);
while (!follower.isFinishedTrajectory() && opModeIsActive()) {
if (time.seconds() > config.dt) {
double distance = (mDrive.getRightDistanceInches() + mDrive.getLeftDistanceInches()) / 2;
double forwardSpeed = follower.calculate(distance);
telemetry.addData("Distance", distance);
telemetry.addData("Turning Error", mDrive.getGyroAngleDegrees() - follower.getHeading());
telemetry.addData("Forward", forwardSpeed);
telemetry.update();
mDrive.setLeftRightPower(forwardSpeed, forwardSpeed);
time.reset();
}
}
mDrive.setLeftRightPower(0, 0);
}
use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Relic_Main by TeamOverdrive.
the class Drive method driveTrajectory.
public void driveTrajectory(double leftDistance, double rightDistance, long timeout) {
int lastLeft = getLeftCurrentPosition();
int lastRight = getRightCurrentPosition();
Trajectory left;
Trajectory right;
left = TrajectoryGenerator.generate(defaultTrajectoryConfig, strategy, 0, 0, Math.abs(leftDistance), 0, 0);
right = TrajectoryGenerator.generate(defaultTrajectoryConfig, strategy, 0, 0, Math.abs(rightDistance), 0, 0);
if (leftDistance < 0) {
left.scale(-1);
}
if (rightDistance < 0) {
right.scale(-1);
}
new FollowPath(linearOpMode, this, new Path("", new Trajectory.Pair(left, right)), timeout, 1, 1);
setRunMode(DcMotor.RunMode.RUN_TO_POSITION);
this.setLeftRightTarget((int) ((leftDistance * COUNTS_PER_INCH) + lastLeft), (int) ((rightDistance * COUNTS_PER_INCH) + lastRight));
setLeftRightPower(0.2, 0.2);
while (linearOpMode.opModeIsActive() && (leftMotor.isBusy() || rightMotor.isBusy())) Thread.yield();
setLeftRightPower(0, 0);
setRunMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
Aggregations