eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Claw, Gantry, and Lift

This commit is contained in:
Thomas 2023-12-01 07:57:36 -06:00
parent c289ad3cc4
commit 2b167a78ca
2 changed files with 62 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
TeamCode/src/main/java/opmodes/RedBackStage.java Normal file
View File

@ -0,0 +1,61 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp")
public class RedBackStage extends AutoBase {
private final Pose2d rendezvous = new Pose2d(12, 11);
public RedBackStage() {
super(
CenterStageCommon.Alliance.Red,
new Pose2d(12, 63, Math.toRadians(-90)),
new Pose2d(62, -62));
}
protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
builder.lineToConstantHeading(new Vector2d(19, 34));
builder.addTemporalMarker(0.5, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(-90));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void propRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
}

2
TeamCode/src/main/java/opmodes/RedFrontStage.java
View File

@ -47,7 +47,7 @@ public class RedFrontStage extends AutoBase {
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(90));
builder.turn(Math.toRadians(-90));
this.robot.getDrive().followTrajectorySequence(builder.build());
}