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

tweaked auto and slides

This commit is contained in:
Robert Teal 2024-01-01 19:07:48 -06:00
parent 4b23276e62
commit 3dcbb50c53
3 changed files with 50 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -75,7 +75,6 @@ public class Robot {
case(0):
macroStartTime = runTime;
arm.setDoor(OPEN);
slides.setTarget(slides.getTarget()+70);
macroState++;
break;
case(1):

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java
View File

@ -5,18 +5,20 @@ import com.arcrobotics.ftclib.controller.PIDController;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
@Config
public class Slides {
private DcMotor slide;
private DcMotor slide2;
public static double p = 0.0014;
public static double i = 0.02;
public static double d = 0;
public static double f = 0.01;
// public static double p = 0.0014;
// public static double i = 0.02;
// public static double d = 0;
// public static double f = 0.01;
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0014,0.02,0,0.01);
public static double pTolerance = 20;
public static PIDController controller = new PIDController(p, i, d);
private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d);
public static int targetMin = -10;
public static int targetMax = 830;
@ -102,15 +104,15 @@ public class Slides {
// slide2.setPower(0);
// } else {
double pid, ff;
controller.setPID(p, i, d);
controller.setPID(coefficients.p, coefficients.i, coefficients.d);
controller.setTolerance(pTolerance);
pid = controller.calculate(slide.getCurrentPosition(), target);
ff = f;
ff = coefficients.f;
slide.setPower(pid + ff);
pid = controller.calculate(slide2.getCurrentPosition(), target);
ff = f;
ff = coefficients.f;
slide2.setPower(pid + ff);
// }
// }

49
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java
View File

@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
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;
@ -20,6 +21,10 @@ public class RedBackStageAuto extends AutoBase {
public Trajectory parkRobot2;
public Trajectory parkRobot3;
public Trajectory stackrun1b1;
public Trajectory stackrun1b2;
public Trajectory stackrun1b3;
@Override
public void createTrajectories() {
// set start position
@ -41,6 +46,10 @@ public class RedBackStageAuto extends AutoBase {
Pose2d park2 = new Pose2d(48, -12, Math.toRadians(180));
Pose2d park3 = new Pose2d(48, -12, Math.toRadians(180));
Pose2d stack_1x1 = new Pose2d(-56, -12, Math.toRadians(180));
Pose2d stack_2x1 = new Pose2d(-56, -12, Math.toRadians(180));
Pose2d stack_3x1 = new Pose2d(-56, -12, Math.toRadians(180));
// create trajectories
scorePurple1 = robot.drive.trajectoryBuilder(start)
.lineToLinearHeading(drop1)
@ -71,6 +80,19 @@ public class RedBackStageAuto extends AutoBase {
parkRobot3 = robot.drive.trajectoryBuilder(scoreYellow3.end())
.lineToLinearHeading(park3)
.build();
stackrun1b1 = robot.drive.trajectoryBuilder(scoreYellow1.end())
.splineToConstantHeading(new Vector2d(30, -12), Math.toRadians(0))
.lineToLinearHeading(stack_1x1)
.build();
stackrun1b2 = robot.drive.trajectoryBuilder(scoreYellow2.end())
.splineToConstantHeading(new Vector2d(30, -12), Math.toRadians(0))
.lineToLinearHeading(stack_2x1)
.build();
stackrun1b3 = robot.drive.trajectoryBuilder(scoreYellow1.end())
.splineToConstantHeading(new Vector2d(30, -12), Math.toRadians(0))
.lineToLinearHeading(stack_1x1)
.build();
}
@Override
@ -117,21 +139,30 @@ public class RedBackStageAuto extends AutoBase {
case 4:
robot.resetMacro(0, getRuntime());
if (robot.macroState >= 2){
robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3));
// robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3));
robot.drive.followTrajectoryAsync(stackrun1b2);
macroState++;
}
break;
// PARK ROBOT
case 5:
// reset macro'
if (robot.macroState != 0) {
robot.resetMacro(0, getRuntime());
}
// if macro and drive are done, end auto
if (robot.macroState == 0 && !robot.drive.isBusy()) {
macroState=-1;
if(!robot.drive.isBusy()){
macroState =-1;
}
//macroState++;
break;
// PARK ROBOT
// case 6:
// // reset macro'
// if (robot.macroState != 0) {
// robot.resetMacro(0, getRuntime());
// }
// // if macro and drive are done, end auto
// if (robot.macroState == 0 && !robot.drive.isBusy()) {
// macroState=-1;
// }
// break;
}
}
}