Skip to content

Commit

Permalink
auto.
Browse files Browse the repository at this point in the history
  • Loading branch information
@richpant
richpant committed Nov 7, 2024
1 parent f555311 commit c565c75
Show file tree
Hide file tree
Showing 5 changed files with 20 additions and 13 deletions.
10 changes: 5 additions & 5 deletions TeamCode/src/main/java/indubitables/config/runmodes/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,19 +187,19 @@ public void buildPaths() {

pushSamples = follower.pathBuilder()
.addPath(new BezierCurve(new Point(preloadPose), new Point(16.088, 22.000, Point.CARTESIAN), new Point(57.345, 50.496, Point.CARTESIAN), new Point(56.000, 26.000, Point.CARTESIAN)))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(90))
.addPath(new BezierLine(new Point(56.000, 26.000, Point.CARTESIAN), new Point(24, 26.000, Point.CARTESIAN)))
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(new BezierCurve(new Point(24, 26.000, Point.CARTESIAN), new Point(56.000, 26.000, Point.CARTESIAN), new Point(56.000, 16.000, Point.CARTESIAN)))
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(new BezierLine(new Point(56.000, 16.000, Point.CARTESIAN),new Point(24, 16.000, Point.CARTESIAN)))
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
// .addPath(new BezierCurve(new Point(24, 16.000, Point.CARTESIAN), new Point(56.000, 16.000, Point.CARTESIAN), new Point(56.000, 9.50, Point.CARTESIAN)))
// .setConstantHeadingInterpolation(Math.toRadians(0))
// .addPath(new BezierLine(new Point(56.000, 9.50, Point.CARTESIAN), new Point(24, 9.5, Point.CARTESIAN)))
// .setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(new BezierLine(new Point(24, 16, Point.CARTESIAN), new Point(blueObservationSpecimenSetPose)))
.setLinearHeadingInterpolation(Math.toRadians(0), blueObservationSpecimenSetPose.getHeading())
.setLinearHeadingInterpolation(Math.toRadians(90), blueObservationSpecimenSetPose.getHeading())
.build();

grab1 = follower.pathBuilder()
Expand Down
9 changes: 6 additions & 3 deletions TeamCode/src/main/java/indubitables/config/subsystem/LiftSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,7 @@ public LiftSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
leftLift.setDirection(DcMotor.Direction.REVERSE);
rightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
leftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);


liftPID = new PIDController(p, i, d);

Expand All @@ -52,6 +51,9 @@ public LiftSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {

public void updatePIDF(){
if (!manual) {
rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);

double pid = liftPID.calculate(getPos(), target);
double ticks_in_degrees = 537.7 / 360.0;
double ff = Math.cos(Math.toRadians(target / ticks_in_degrees)) * f;
Expand All @@ -66,7 +68,8 @@ public void updatePIDF(){
}

public void manual(double n){
manual = true;
rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

rightLift.setPower(n);
leftLift.setPower(n);
Expand Down
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/indubitables/config/util/FieldConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@ public enum RobotStart {
public static final Pose blueObservationElement3Pose = new Pose(20, 96, Math.toRadians(180));
public static final Pose blueObservationElement3ControlPose = new Pose(20, 110);
public static final Pose blueObservationSpecimenSetPose = new Pose(21, 25.5, Math.toRadians(0));
public static final Pose blueObservationSpecimenPickupPose = new Pose(16.5, 25.75, Math.toRadians(0));
public static final Pose blueObservationSpecimenPickup2Pose = new Pose(16.75, 25.5, Math.toRadians(0));
public static final Pose blueObservationSpecimenPickupPose = new Pose(17.25, 25.5, Math.toRadians(0));
public static final Pose blueObservationSpecimenPickup2Pose = new Pose(17.25, 25.5, Math.toRadians(0));
public static final Pose blueObservationSpecimen1Pose = new Pose(28, 68.625, Math.toRadians(180));
public static final Pose blueObservationSpecimen2Pose = new Pose(28.25, 71.625, Math.toRadians(180));
public static final Pose blueObservationSpecimen3Pose = new Pose(30.25, 76.625, Math.toRadians(180));
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/indubitables/opmode/BlueObservation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ public void pathUpdate() {
if(pathTimer.getElapsedTimeSeconds() > 0.5) {
auto.init();
auto.liftPIDF = false;
auto.liftManual = -0.25;
auto.liftManual = -0.15;
setPathState(13); }
break;
case 13: //Drives to chamber once action finishes
Expand Down
8 changes: 6 additions & 2 deletions TeamCode/src/main/java/indubitables/opmode/pidTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,8 +61,6 @@ public void init() {
rightLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
leftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

bottom = getPos();

Expand Down Expand Up @@ -90,11 +88,17 @@ public void loop() {
telemetry.addData("pos", getPos());
telemetry.addData("baron", baron);
telemetry.addData("target", target);
telemetry.addData("P:", p);
telemetry.addData("I:", i);
telemetry.addData("D:", d);
telemetry.addData("F:", f);
telemetry.update();
}

public void updatePIDF(){
if (!manual) {
rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
double pid = liftPID.calculate(getPos(), target);
double ticks_in_degrees = 537.7 / 360.0;
double ff = Math.cos(Math.toRadians(target / ticks_in_degrees)) * f;
Expand Down

0 comments on commit c565c75

Please sign in to comment.