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

Tuned roadrunner, refactored a bunch of things, added macros, added a basic auto, added a new teleop

This commit is contained in:
Robert Teal 2023-11-20 23:36:55 -06:00
parent 7610f97542
commit 5329732517
16 changed files with 925 additions and 576 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
TeamCode/build.gradle
View File

@ -10,27 +10,11 @@
// Custom definitions may go here
// Include common definitions from above.
apply from: '../build.common.gradle'
apply from: '../build.dependencies.gradle'
android {
defaultConfig {
externalNativeBuild {
cmake {
cppFlags ''
}
}
}
namespace = 'org.firstinspires.ftc.teamcode'
externalNativeBuild {
cmake {
path file('src/main/cpp/CMakeLists.txt')
version '3.18.1'
}
}
packagingOptions {
jniLibs.useLegacyPackaging true

75
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
View File

@ -21,8 +21,8 @@ public class Arm {
private double ARM_KP = 0.001;
public enum APosition {
SDOWN, SCORE, SAFTEYDOWN, SAFTEYUP;
public enum Position {
INTAKE, SCORE;
}
public Arm(HardwareMap hardwareMap) {
@ -34,17 +34,36 @@ public class Arm {
rAServo.setDirection(Servo.Direction.REVERSE);
dServo.setDirection(Servo.Direction.REVERSE);
// wristServo.setDirection(Servo.Direction.REVERSE);
setArmPos(Position.INTAKE);
setWristPos(Position.INTAKE);
}
public static double armScorePos = 1;
public static double doorOpenpos = 0.5;
public static double doorClosePos = 0.85;
public static double startingWristPos = 0.735;
public static double startingArmPos = 0.325;
public static double safteyDownPos = 0.4;
public static double safteyUpPos = 0.9;
public static double wristScorePos = 0.98;
public static double safeWrist = 0.8;
public static double armStart = 0.325;
public static double armScore = 1;
public static double wristStart = 0.735;
public static double wristScore = 0.98;
public void setArmPos(Position pos) {
if (pos == Position.INTAKE) {
rAServo.setPosition(armStart);
lAServo.setPosition(armStart);
} else if (pos == Position.SCORE) {
rAServo.setPosition(armScore);
lAServo.setPosition(armScore);
}
}
public void setWristPos(Position pos) {
if (pos == Position.INTAKE) {
wristServo.setPosition(wristStart);
} else if (pos == Position.SCORE) {
wristServo.setPosition(wristScore);
}
}
public void openDoor(DoorPos.DoorPosition pos) {
if (pos == DoorPos.DoorPosition.OPEN) {
@ -54,48 +73,10 @@ public class Arm {
}
}
public void setPos(APosition tape) {
if (tape == APosition.SDOWN) {
this.armControllerTarget = startingArmPos;
lAServo.setPosition(startingArmPos);
rAServo.setPosition(startingArmPos);
wristServo.setPosition(startingWristPos);
} else if (tape == APosition.SCORE) {
this.armControllerTarget = armScorePos;
lAServo.setPosition(armScorePos);
rAServo.setPosition(armScorePos);
wristServo.setPosition(wristScorePos);
} else if (tape == APosition.SAFTEYDOWN) {
lAServo.setPosition(safteyDownPos);
rAServo.setPosition(safteyDownPos);
wristServo.setPosition(wristScorePos);
} else if (tape == APosition.SAFTEYUP) {
lAServo.setPosition(safteyUpPos);
rAServo.setPosition(safteyUpPos);
wristServo.setPosition(wristScorePos);
}
}
// public void setHopper(HopperPos.hopperPos hopper) {
// if (hopper == HopperPos.hopperPos.UP) {
// wristServo.setPosition(wristScorePos);
// } else if (hopper == HopperPos.hopperPos.DOWN) {
// wristServo.setPosition(startingWristPos);
// }
// }
public String getTelemetry() {
return "Wrist Servo: "+wristServo.getPosition()+"Left Arm Servo: "+lAServo.getPosition()+"Right Arm Servo: "+rAServo.getPosition()+"Door Servo: "+dServo.getPosition();
}
public void update() {
armController.setP(0.01);
this.armController.setSetPoint(this.armControllerTarget);
double output = this.armController.calculate(this.lAServo.getPosition());
this.lAServo.setPosition(this.lAServo.getPosition() + output);
this.rAServo.setPosition(this.rAServo.getPosition() + output);
}
}

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java
View File

@ -11,6 +11,7 @@ public class Intake {
private Servo lServo;
private DcMotor dcMotor;
private Position pos = Position.UP;
public enum Position {
STACK1, STACK2, STACK3, STACK4, STACK5, UP;
@ -63,6 +64,14 @@ public class Intake {
}
}
public void incrementPos() {
pos = pos.nextPosition();
}
public void decrementPos() {
pos = pos.previousPosition();
}
public void setDcMotor(double pwr) {
dcMotor.setPower(pwr);
}

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

@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder;
@ -15,102 +16,78 @@ public class Robot {
public SampleMecanumDrive drive;
public Camera camera;
public Intake intake;
public KhangSlides slides;
public Slides slides;
public Arm arm;
public double macroStartTime = 0;
public int macroState = 0;
public int runningMacro = 0; // 0 = no macro | 1 = low macro | 2 = mid macro | 3 = high macro | 4 = pickup macro
public int runningMacro = 0; // 0 = no macro | 1 = tier 1 | 2 = tier 2 | 3 = tier 3 | 4 = return
public int lastMacro = 0;
private boolean camEnabled = true;
//Name the objects
public Robot(HardwareMap hardwareMap) {
arm = new Arm(hardwareMap);
drive = new SampleMecanumDrive(hardwareMap);
slides = new KhangSlides(hardwareMap);
camera = new Camera(hardwareMap);
camera.initTargetingCamera();
intake = new Intake(hardwareMap, UP);
slides = new Slides(hardwareMap);
arm = new Arm(hardwareMap);
camEnabled = true;
}
}
public void extendMacro(Slides.Position slidePos, double runTime) {
switch(macroState) {
case(0):
macroStartTime = runTime;
slides.setTarget(slidePos);
macroState ++;
break;
case(1):
if (runTime > macroStartTime + 1) {
macroState ++;
}
break;
case(2):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.SCORE);
arm.setWristPos(Arm.Position.SCORE);
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
break;
}
}
public void resetMacro(Slides.Position pos, double runTime) {
switch(macroState) {
case(0):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.INTAKE);
arm.setWristPos(Arm.Position.INTAKE);
macroState++;
break;
case(1):
if (runTime > macroStartTime + 1) {
macroState ++;
}
break;
case(2):
macroStartTime = runTime;
slides.setTarget(pos);
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
}
}
//Update on runtime and dive
public void update(double runTime) {
drive.update();
slides.update(runTime);
arm.update();
}
//Return position to control hub
public String getTelemetry() {
Encoder slide = null;
return String.format("position: %s", slide.getCurrentPosition());
}
// public void resetMacroNoDunk(int pos, double runTime) {
// switch(macroState) {
// case(0):
// macroStartTime = runTime;
// macroState++;
// break;
// case(1):
// if (runTime > macroStartTime) {
// macroState ++;
// }
// break;
// case(2):
// macroStartTime = runTime;
// slides.setTarget(pos);
// macroState = 0;
// runningMacro = 0;
// lastMacro = 0;
// }
// }
//
// public void resetMacro(int pos, double runTime) {
// switch(macroState) {
// case(0):
// macroStartTime = runTime;
// macroState++;
// break;
// case(1):
// if (runTime > macroStartTime) {
// macroState ++;
// }
// break;
// case(2):
// macroStartTime = runTime;
// slides.setTarget(pos);
// macroState = 0;
// runningMacro = 0;
// lastMacro = 0;
// }
// }
//
// public void resetMacroEnd(int pos, double runTime) {
// switch(macroState) {
// case(0):
// macroStartTime = runTime;
// macroState++;
// break;
// case(1):
// if (runTime > macroStartTime) {
// macroState ++;
// }
// break;
// case(2):
// macroStartTime = runTime;
// slides.setTarget(pos);
// macroState = 0;
// runningMacro = 0;
// lastMacro = 0;
// }
// }
public Arm getArm() {
return this.arm;
}
//return drive
public SampleMecanumDrive getDrive() {
return this.drive;
return "Telemetry?";
}
}

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

@ -18,33 +18,28 @@ public class Slides {
public static double pTolerance = 20;
public static PIDController controller = new PIDController(p, i, d);
public static int[] heights = {0, (int)(190/4.0), 2*(int)(190/4.0), 3*(int)(190/4.0), 4*(int)(180/4)};
public static int heightOffset = 0;
public static int targetMin = -10;
public static int targetMax = 770;
public static int targetMax = 1000;
public static int highPos = 720 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES
public static int midPos = 350 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES
public static int lowPos = heightOffset; // ALSO DEFINED IN UPDATE SLIDES
public static int pickupPos = 220 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES
public static int downPos = heightOffset;
public static int down = 0;
public static int tier1 = 200;
public static int tier2 = 350;
public static int tier3 = 500;
private int target = 0;
public static int manualSpeed = 20;
public static int zeroPower = 5;
public enum Position { HIGH, MEDIUM, LOW, PICKUP, DOWN }
public enum Position { DOWN, TIER1, TIER2, TIER3 }
public Slides(HardwareMap hardwareMap) {
slide = hardwareMap.get(DcMotor.class, "slide");
slide = hardwareMap.get(DcMotor.class, "Right Slide Motor");
slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
slide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// slide.setDirection(DcMotorSimple.Direction.REVERSE);
slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
slide2 = hardwareMap.get(DcMotor.class, "slide2");
slide2 = hardwareMap.get(DcMotor.class, "Left Slide Motor");
slide2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
slide2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
slide2.setDirection(DcMotorSimple.Direction.REVERSE);
@ -56,16 +51,14 @@ public class Slides {
}
public void setTarget(Position pos) {
if (pos == Position.HIGH) {
target = Math.min(Math.max(highPos, targetMin), targetMax);
} else if (pos == Position.MEDIUM) {
target = Math.min(Math.max(midPos, targetMin), targetMax);
} else if (pos == Position.LOW) {
target = Math.min(Math.max(lowPos, targetMin), targetMax);
} else if (pos == Position.PICKUP) {
target = Math.min(Math.max(pickupPos, targetMin), targetMax);
} else if (pos == Position.DOWN) {
target = Math.min(Math.max(downPos, targetMin), targetMax);
if (pos == Position.DOWN) {
target = Math.min(Math.max(down, targetMin), targetMax);
} else if (pos == Position.TIER1) {
target = Math.min(Math.max(tier1, targetMin), targetMax);
} else if (pos == Position.TIER2) {
target = Math.min(Math.max(tier2, targetMin), targetMax);
} else if (pos == Position.TIER3) {
target = Math.min(Math.max(tier3, targetMin), targetMax);
}
}
@ -121,6 +114,6 @@ public class Slides {
}
public String getTelemetry() {
return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s\nHeightOffset: %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower(), heightOffset);
return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s\nHeightOffset: %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower());
}
}

291
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java Normal file
View File

@ -0,0 +1,291 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
import org.firstinspires.ftc.teamcode.util.CameraPosition;
import java.util.ArrayList;
import java.util.Locale;
public abstract class AbstractAuto extends LinearOpMode {
public Robot robot;
private int teamElementLocation = 2;
private ArrayList<Step> steps;
private double currentRuntime;
public CameraPosition cameraPosition;
// Main method to run all the steps for autonomous
@Override
public void runOpMode() {
// telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Initializing Robot...");
telemetry.update();
setCameraPosition();
robot = new Robot(hardwareMap);
makeTrajectories();
// while (robot.camera.getFrameCount() < 1) {
// idle();
// }
// wait for start
while (!(isStarted() || isStopRequested())) {
currentRuntime = getRuntime();
robot.update(currentRuntime);
// int newLocation = robot.camera.getMarkerId();
// if (newLocation != -1) {
// teamElementLocation = newLocation;
// }
telemetry.addLine("Initialized");
telemetry.addLine("Randomization: "+teamElementLocation);
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
resetRuntime();
// build the first step
steps = new ArrayList<>();
// stopCamera();
initializeSteps(teamElementLocation);
int stepNumber = 0;
double stepTimeout;
Step step = steps.get(stepNumber);
stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
step.start();
// run the remaining steps
while (opModeIsActive()) {
currentRuntime = getRuntime();
// once a step finishes
if (step.isFinished() || currentRuntime >= stepTimeout) {
// do the finishing move
step.end();
stepNumber++;
// if it was the last step break out of the while loop
if (stepNumber > steps.size() - 1) {
break;
}
// else continue to the next step
step = steps.get(stepNumber);
stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
step.start();
}
// // update turret and slides position
// PoseStorage.slidesPosition = robot.actuators.getSlides();
// PoseStorage.turretPosition = robot.actuators.getTurret();
// while the step is running display telemetry
step.whileRunning();
robot.update(currentRuntime);
telemetry.addLine(String.format(Locale.US, "Runtime: %.0f", currentRuntime));
telemetry.addLine("Step " + (stepNumber + 1) + " of " + steps.size() + ", " + step.getTelemetry() + "\n");
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}
// Load up all of the steps for the autonomous: to be overridden with the specific steps in the specific auto
public abstract void initializeSteps(int location);
//methods to be implemented in the specific autos
public abstract void setAlliance();
public abstract void setCameraPosition();
public abstract boolean useCamera();
public abstract void makeTrajectories();
// public abstract void setArm(Arm.Position armPos, Claw.Position clawPos);
//other methods that do certain tasks
public void turn(double degrees) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.turn(degrees);
}
@Override
public void whileRunning() {
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy();
}
});
}
public void followTrajectory(Trajectory trajectory) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
}
@Override
public void whileRunning() {
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy();
}
});
}
public void followAndExtend(Trajectory trajectory, Slides.Position slidePos) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
if (slidePos == Slides.Position.TIER1) {
robot.runningMacro = 1;
} else if (slidePos == Slides.Position.TIER2) {
robot.runningMacro = 2;
} else if (slidePos == Slides.Position.TIER3) {
robot.runningMacro = 3;
}
robot.extendMacro(slidePos, currentRuntime);
}
@Override
public void whileRunning() {
if (robot.runningMacro != 0) {
robot.extendMacro(slidePos, currentRuntime);
}
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy() && robot.runningMacro == 0;
}
});
}
public void followAndRetract(Trajectory trajectory, Slides.Position slidePos) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
robot.runningMacro = 4;
robot.resetMacro(slidePos, currentRuntime);
}
@Override
public void whileRunning() {
if (robot.runningMacro != 0) {
robot.resetMacro(slidePos, currentRuntime);
}
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy() && robot.runningMacro == 0;
}
});
}
public void runIntake(double power, double timeout) {
steps.add(new Step("Running Intake" + timeout + " seconds", timeout) {
@Override
public void start() {
robot.intake.setDcMotor(power);
}
@Override
public void whileRunning() {
}
@Override
public void end() {
robot.intake.setDcMotor(0);
}
@Override
public boolean isFinished() {
return false;
}
});
}
// Functions to add steps
public void addDelay(double timeout) {
steps.add(new Step("Waiting for " + timeout + " seconds", timeout) {
@Override
public void start() {
}
@Override
public void whileRunning() {
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return false;
}
});
}
public void stopCamera() {
steps.add(new Step("Stopping Signal Camera") {
@Override
public void start() {
robot.camera.stopTargetingCamera();
}
@Override
public void whileRunning() {
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return true;
}
});
}
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java Normal file
View File

@ -0,0 +1,59 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
@Autonomous(name = "Blue Left Auto")
public class BlueLeftAuto extends AbstractAuto {
public Trajectory scorePurple;
public Trajectory scoreYellow;
public Trajectory parkRobot;
@Override
public void initializeSteps(int location) {
followTrajectory(scorePurple);
runIntake(-0.3, 1);
followAndExtend(scoreYellow, Slides.Position.TIER1);
followAndRetract(parkRobot, Slides.Position.DOWN);
}
@Override
public void setAlliance() {
}
@Override
public void setCameraPosition() {
}
@Override
public boolean useCamera() {
return false;
}
@Override
public void makeTrajectories() {
Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d dropMiddle = new Pose2d(24, 40.5, Math.toRadians(-90));
// Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d score = new Pose2d(62, 36, Math.toRadians(180));
Pose2d park = new Pose2d(62, 60, Math.toRadians(180));
scorePurple = robot.drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
scoreYellow = robot.drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
parkRobot = robot.drive.trajectoryBuilder(scoreYellow.end())
.lineToLinearHeading(park)
.build();
robot.drive.setPoseEstimate(start);
}
}

27
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BotGoMove.java
View File

@ -1,27 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous
public class BotGoMove extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory myTrajectory = drive.trajectoryBuilder(new Pose2d())
.forward(3.7)
.build();
waitForStart();
if(isStopRequested()) return;
drive.followTrajectory(myTrajectory);
}
}

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto.java Normal file
View File

@ -0,0 +1,42 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous(name = "Simple Blue Left Auto")
public class SimpleBlueLeftAuto extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d dropMiddle = new Pose2d(24, 40.5, Math.toRadians(-90));
// Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d score = new Pose2d(60, 36, Math.toRadians(180));
drive.setPoseEstimate(start);
Trajectory scorePurple = drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
waitForStart();
if(isStopRequested()) return;
drive.followTrajectory(scorePurple);
// drive.wait(1);
drive.followTrajectory(scoreYellow);
}
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java
View File

@ -1,59 +0,0 @@
//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;
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import org.firstinspires.ftc.teamcode.hardware.Camera;
//import org.firstinspires.ftc.teamcode.hardware.Intake;
//import org.firstinspires.ftc.teamcode.hardware.Robot;
//import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants;
//import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
//import org.firstinspires.ftc.teamcode.util.CameraPosition;
//
//@Autonomous(name = "Start From Left Wall", group = "Left Start", preselectTeleOp = "Khang Main")
//public class StartFromLeftCenterSpike extends AbstractAuto {
//
// public Robot robot;
// public CameraPosition cameraPosition;
// //Steps
// public Trajectory start;
//
// @Override
// public void initializeSteps(int location) {
// followTrajectory(start);
// }
//
// @Override
// public void setAlliance() {}
//
// @Override
// public void setCameraPosition() {
// cameraPosition = CameraPosition.FRONT;
// }
//
// @Override
// public boolean useCamera() {
// return true;
// }
//
// @Override
// public void waitForStart() {
// super.waitForStart();
// }
//
// @Override
// public void makeTrajectories() {
//
// // positions
// Pose2d start = new Pose2d(-65.125,-48,Math.toRadians(90));
// Vector2d step1 = new Vector2d(-24,-48);
// Pose2d step2 = new Pose2d(-24,-48,Math.toRadians(90));
//
//// this.start = robot.drive.trajectoryBuilder(start)
// this.start = robot.drive.trajectoryBuilder(new Pose2d())
// .forward(3)
// .build();
// }
//}

106
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java
View File

@ -1,53 +1,53 @@
//package org.firstinspires.ftc.teamcode.opmode.autonomous;
//
//
//import org.firstinspires.ftc.teamcode.vision.Detection;
//
//// Class for every step that the autonomous program will take
//public abstract class Step {
// private final double timeout;
// private String telemetry;
//
// // variables when moving
// public double x;
// public double y;
// public double stepTime;
// double tempTime = stepTime;
//
// // variables when shooting
// public Detection teamProp;
//
// // Constructors
// public Step(String telemetry) {
// this.telemetry = telemetry;
// this.timeout = -1;
// }
//
// public Step(String telemetry, double timeout) {
// this.telemetry = telemetry;
// this.timeout = timeout;
// }
//
// // Abstract methods to be overrode
// public abstract void start();
//
// public abstract void whileRunning();
//
// public abstract void end();
//
// public abstract boolean isFinished();
//
// // Return the timeout for the step
// public double getTimeout() {
// return timeout;
// }
//
// public void setTelemetry(String telemetry) {
// this.telemetry = telemetry;
// }
//
// // Return the Telemetry for the step
// public String getTelemetry() {
// return telemetry;
// }
//}
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import org.firstinspires.ftc.teamcode.vision.Detection;
// Class for every step that the autonomous program will take
public abstract class Step {
private final double timeout;
private String telemetry;
// variables when moving
public double x;
public double y;
public double stepTime;
double tempTime = stepTime;
// variables when shooting
public Detection teamProp;
// Constructors
public Step(String telemetry) {
this.telemetry = telemetry;
this.timeout = -1;
}
public Step(String telemetry, double timeout) {
this.telemetry = telemetry;
this.timeout = timeout;
}
// Abstract methods to be overrode
public abstract void start();
public abstract void whileRunning();
public abstract void end();
public abstract boolean isFinished();
// Return the timeout for the step
public double getTimeout() {
return timeout;
}
public void setTelemetry(String telemetry) {
this.telemetry = telemetry;
}
// Return the Telemetry for the step
public String getTelemetry() {
return telemetry;
}
}

504
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java
View File

@ -1,260 +1,260 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Arm;
import org.firstinspires.ftc.teamcode.hardware.DoorPos;
import org.firstinspires.ftc.teamcode.hardware.Down;
import org.firstinspires.ftc.teamcode.hardware.Hieght;
import org.firstinspires.ftc.teamcode.hardware.HopperPos;
import org.firstinspires.ftc.teamcode.hardware.Intake;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.SlidePos;
import com.qualcomm.robotcore.util.ElapsedTime;
@Config
@TeleOp(name = "Meet 1 TeleOp", group = "OpModes")
public class Meet1TeleOp extends OpMode {
//turbo mode
public static double normal = 0.5;
public static double turbo = 1;
//keep track of runtime for advanced macros
private ElapsedTime runTime = new ElapsedTime();
//create and set start position of intake
private Intake.Position Currentpos = Intake.Position.UP;
//inform if slides are in lowest position or not
private Down.DownArm DownQuestion = Down.DownArm.YES;
//create and set start position of slides
private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN;
//create and set start position of arm
private Arm.APosition CurrentApos = Arm.APosition.SDOWN;
//create and set start position of door, default close
private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE;
//create and set start position of hopper
private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN;
//create manual slide height varable and set it to hold position as start
private Hieght.height CurrentHeight = Hieght.height.HOLD;
//create robot instance
private Robot robot;
//create servo for plane
private Servo planeServo;
//create controller 1 (driver)
private Controller controller1;
//create controller 2 (macro user and one with hard life)
private Controller controller2;
//set starting slide height to 0
private double sHeight = 0;
@Override
public void init() {
//reset runtime when opmode is initialized
runTime.reset();
//make each servo, motor, and controller and configure them
planeServo = hardwareMap.get(Servo.class, "Plane Servo");
this.robot = new Robot(hardwareMap);
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
//feedback to driver hub to know if init was successful
telemetry.addLine("Started");
//update to make sure we receive last line of code
telemetry.update();
}
@Override
public void loop() {
// Calculate the runtime in seconds
double currentTime = runTime.seconds();
//set start of macro runtime
double macroStartTime = 0;
//create and set X, Y, and Z for drive inputs
double x = -gamepad1.left_stick_y;
double y = -gamepad1.left_stick_x;
double z = -gamepad1.right_stick_x;
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
//turbo activation
if (controller1.getA().isJustPressed()){
x *= turbo;
y *= turbo;
z *= turbo;
} else if (controller1.getA().isJustReleased()){
x *= normal;
y *= normal;
z *= normal;
}
//set intake to be pressure reactant to right trigger
robot.intake.setDcMotor(gamepad2.right_trigger);
//manual slide height control
double sHeight = gamepad2.right_stick_y;
//activate intake or not
double intakeON = gamepad2.right_trigger;
//graph input of Y joystick to make macros for slides
double sY = -gamepad2.left_stick_y;
//graph of X
double sX = gamepad2.left_stick_x;
double manualSY = gamepad2.right_stick_y;
//reset value to set slides to starting value
double reset = gamepad2.left_trigger;
//variable to determine shoot drone or not
double shoot = gamepad1.right_trigger;
//update variables to constantly feed position each component should be in
//intake
robot.intake.setpos(Currentpos);
//slide macro
robot.slides.setSPos(CurrentSpos);
//slide manual
// robot.slides.setHeight(CurrentHeight);
//arm position
robot.arm.setPos(CurrentApos);
//door open or not
robot.arm.openDoor(CurrentDpos);
//update
controller2.update();
//manual slide input reader
// if (sHeight >= 0.2) {
// CurrentHeight = Hieght.height.ASCEND;
// } else if (sHeight <= -0.2) {
// CurrentHeight = Hieght.height.DESCEND;
//package org.firstinspires.ftc.teamcode.opmode.teleop;
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//import com.qualcomm.robotcore.hardware.Servo;
//
//import org.firstinspires.ftc.teamcode.controller.Controller;
//import org.firstinspires.ftc.teamcode.hardware.Arm;
//import org.firstinspires.ftc.teamcode.hardware.DoorPos;
//import org.firstinspires.ftc.teamcode.hardware.Down;
//import org.firstinspires.ftc.teamcode.hardware.Hieght;
//import org.firstinspires.ftc.teamcode.hardware.HopperPos;
//import org.firstinspires.ftc.teamcode.hardware.Intake;
//import org.firstinspires.ftc.teamcode.hardware.Robot;
//import org.firstinspires.ftc.teamcode.hardware.SlidePos;
//import com.qualcomm.robotcore.util.ElapsedTime;
//
//@Config
//@TeleOp(name = "Meet 1 TeleOp", group = "OpModes")
//public class Meet1TeleOp extends OpMode {
//
// //turbo mode
// public static double normal = 0.5;
// public static double turbo = 1;
//
// //keep track of runtime for advanced macros
// private ElapsedTime runTime = new ElapsedTime();
//
// //create and set start position of intake
// private Intake.Position Currentpos = Intake.Position.UP;
// //inform if slides are in lowest position or not
// private Down.DownArm DownQuestion = Down.DownArm.YES;
// //create and set start position of slides
// private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN;
// //create and set start position of arm
// private Arm.Position CurrentApos = Arm.Position.SDOWN;
// //create and set start position of door, default close
// private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE;
// //create and set start position of hopper
// private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN;
// //create manual slide height varable and set it to hold position as start
// private Hieght.height CurrentHeight = Hieght.height.HOLD;
// //create robot instance
// private Robot robot;
// //create servo for plane
// private Servo planeServo;
// //create controller 1 (driver)
// private Controller controller1;
// //create controller 2 (macro user and one with hard life)
// private Controller controller2;
// //set starting slide height to 0
// private double sHeight = 0;
//
// @Override
// public void init() {
//
// //reset runtime when opmode is initialized
// runTime.reset();
//
// //make each servo, motor, and controller and configure them
// planeServo = hardwareMap.get(Servo.class, "Plane Servo");
// this.robot = new Robot(hardwareMap);
// controller1 = new Controller(gamepad1);
// controller2 = new Controller(gamepad2);
// //feedback to driver hub to know if init was successful
// telemetry.addLine("Started");
// //update to make sure we receive last line of code
// telemetry.update();
// }
//
// @Override
// public void loop() {
//
// // Calculate the runtime in seconds
// double currentTime = runTime.seconds();
//
// //set start of macro runtime
// double macroStartTime = 0;
//
// //create and set X, Y, and Z for drive inputs
// double x = -gamepad1.left_stick_y;
// double y = -gamepad1.left_stick_x;
// double z = -gamepad1.right_stick_x;
// robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
//
// //turbo activation
// if (controller1.getA().isJustPressed()){
// x *= turbo;
// y *= turbo;
// z *= turbo;
// } else if (controller1.getA().isJustReleased()){
// x *= normal;
// y *= normal;
// z *= normal;
// }
//
// //set intake to be pressure reactant to right trigger
// robot.intake.setDcMotor(gamepad2.right_trigger);
// //manual slide height control
// double sHeight = gamepad2.right_stick_y;
// //activate intake or not
// double intakeON = gamepad2.right_trigger;
// //graph input of Y joystick to make macros for slides
// double sY = -gamepad2.left_stick_y;
// //graph of X
// double sX = gamepad2.left_stick_x;
// double manualSY = gamepad2.right_stick_y;
//
// //reset value to set slides to starting value
// double reset = gamepad2.left_trigger;
//
// //variable to determine shoot drone or not
// double shoot = gamepad1.right_trigger;
//
// //update variables to constantly feed position each component should be in
// //intake
// robot.intake.setpos(Currentpos);
// //slide macro
// robot.slides.setSPos(CurrentSpos);
// //slide manual
//// robot.slides.setHeight(CurrentHeight);
// //arm position
// robot.arm.setArmPos(CurrentApos);
// //door open or not
// robot.arm.openDoor(CurrentDpos);
// //update
// controller2.update();
//
// //manual slide input reader
//// if (sHeight >= 0.2) {
//// CurrentHeight = Hieght.height.ASCEND;
//// } else if (sHeight <= -0.2) {
//// CurrentHeight = Hieght.height.DESCEND;
//// } else {
//// CurrentHeight = Hieght.height.HOLD;
//// }
//
// //read values to determine if plane should shoot or not
// if (shoot >= 0.9) {
// planeServo.setPosition(0.2);
// } else {
// CurrentHeight = Hieght.height.HOLD;
// planeServo.setPosition(0);
// }
//read values to determine if plane should shoot or not
if (shoot >= 0.9) {
planeServo.setPosition(0.2);
} else {
planeServo.setPosition(0);
}
//make door rise as intake goes on
if (intakeON >= 0.01) {
CurrentDpos = DoorPos.DoorPosition.OPEN;
} else {
CurrentDpos = DoorPos.DoorPosition.CLOSE;
}
//make intake go to stack 1 when pressed down
if (intakeON >= 0.25) {
Currentpos = Intake.Position.STACK1;
} else {
Currentpos = Intake.Position.UP;
}
//reset slide position
if (reset >= 0.2) {
CurrentSpos = CurrentSpos.DOWN;
}
// //control position of hopper
// if (controller2.getLeftBumper().isJustPressed()) {
// hopperpos = HopperPos.hopperPos.UP;
// } else if (controller2.getLeftBumper().isJustReleased()) {
// hopperpos = HopperPos.hopperPos.DOWN;
//
// //make door rise as intake goes on
// if (intakeON >= 0.01) {
// CurrentDpos = DoorPos.DoorPosition.OPEN;
// } else {
// CurrentDpos = DoorPos.DoorPosition.CLOSE;
// }
//shift intake up one pixel
if (controller2.getDLeft().isJustPressed()) {
//prevent from going higher than highest legal value
if (Currentpos != Intake.Position.UP) {
Currentpos = Currentpos.nextPosition();
}
}
//shift intake down one pixel
if (controller2.getDRight().isJustPressed()) {
//prevent from going lower than lowest value
if (Currentpos != Intake.Position.STACK1) {
Currentpos = Currentpos.previousPosition();
}
}
//set intake to max position
if (controller2.getDUp().isJustPressed()) {
Currentpos = Currentpos.UP;
}
//set intake to lowest position
if (controller2.getDDown().isJustPressed()) {
Currentpos = Currentpos.STACK1;
}
//arm safety pause going up
if (controller2.getA().isJustPressed()){
CurrentApos = CurrentApos.SAFTEYUP;
DownQuestion = DownQuestion.NO;
}
//arm safety pause going down
if (controller2.getX().isJustPressed()) {
CurrentApos = CurrentApos.SAFTEYDOWN;
DownQuestion = DownQuestion.NO;
}
//arm all the way up
if (controller2.getB().isJustPressed()) {
CurrentApos = CurrentApos.SCORE;
DownQuestion = DownQuestion.NO;
}
//arm all the way down
if (controller2.getY().isJustPressed()) {
CurrentApos = CurrentApos.SDOWN;
DownQuestion = DownQuestion.YES;
}
//set slides to max when left joystick is up
if (sY >= 0.5) {
CurrentSpos = CurrentSpos.MAX;
}
//set slides to tape 1 level when left joystick is down
if (sY <= -0.5) {
CurrentSpos = CurrentSpos.TAPE1;
}
//set slides to tape 3 when left joystick is right
if (sX >= 0.2) {
CurrentSpos = CurrentSpos.TAPE3;
}
//set slides to tape 2 when left joystick is left
if (sX <= -0.2) {
CurrentSpos = CurrentSpos.TAPE2;
}
if (manualSY >=0.1) {
CurrentSpos = CurrentSpos.MANUAL;
}
// //set slides all the way down when left bumper gets pressed
// if (controller2.getLeftBumper().isJustPressed()) {
//
// //make intake go to stack 1 when pressed down
// if (intakeON >= 0.25) {
// Currentpos = Intake.Position.STACK1;
// } else {
// Currentpos = Intake.Position.UP;
// }
//
// //reset slide position
// if (reset >= 0.2) {
// CurrentSpos = CurrentSpos.DOWN;
// }
//
// //set intake all teh way up when right bumper is pressed
// if (controller2.getRightBumper().isJustPressed()) {
//// //control position of hopper
//// if (controller2.getLeftBumper().isJustPressed()) {
//// hopperpos = HopperPos.hopperPos.UP;
//// } else if (controller2.getLeftBumper().isJustReleased()) {
//// hopperpos = HopperPos.hopperPos.DOWN;
//// }
//
// //shift intake up one pixel
// if (controller2.getDLeft().isJustPressed()) {
// //prevent from going higher than highest legal value
// if (Currentpos != Intake.Position.UP) {
// Currentpos = Currentpos.nextPosition();
// }
// }
//
// //shift intake down one pixel
// if (controller2.getDRight().isJustPressed()) {
// //prevent from going lower than lowest value
// if (Currentpos != Intake.Position.STACK1) {
// Currentpos = Currentpos.previousPosition();
// }
// }
//
// //set intake to max position
// if (controller2.getDUp().isJustPressed()) {
// Currentpos = Currentpos.UP;
// }
// update the runtime on the telemetry
telemetry.addData("Runtime", currentTime);
telemetry.update();
}
}
//
// //set intake to lowest position
// if (controller2.getDDown().isJustPressed()) {
// Currentpos = Currentpos.STACK1;
// }
//
// //arm safety pause going up
// if (controller2.getA().isJustPressed()){
// CurrentApos = CurrentApos.SAFTEYUP;
// DownQuestion = DownQuestion.NO;
// }
//
// //arm safety pause going down
// if (controller2.getX().isJustPressed()) {
// CurrentApos = CurrentApos.SAFTEYDOWN;
// DownQuestion = DownQuestion.NO;
// }
//
// //arm all the way up
// if (controller2.getB().isJustPressed()) {
// CurrentApos = CurrentApos.SCORE;
// DownQuestion = DownQuestion.NO;
// }
//
// //arm all the way down
// if (controller2.getY().isJustPressed()) {
// CurrentApos = CurrentApos.SDOWN;
// DownQuestion = DownQuestion.YES;
// }
//
// //set slides to max when left joystick is up
// if (sY >= 0.5) {
// CurrentSpos = CurrentSpos.MAX;
// }
//
// //set slides to tape 1 level when left joystick is down
// if (sY <= -0.5) {
// CurrentSpos = CurrentSpos.TAPE1;
// }
//
// //set slides to tape 3 when left joystick is right
// if (sX >= 0.2) {
// CurrentSpos = CurrentSpos.TAPE3;
// }
//
// //set slides to tape 2 when left joystick is left
// if (sX <= -0.2) {
// CurrentSpos = CurrentSpos.TAPE2;
// }
//
// if (manualSY >=0.1) {
// CurrentSpos = CurrentSpos.MANUAL;
// }
//
//// //set slides all the way down when left bumper gets pressed
//// if (controller2.getLeftBumper().isJustPressed()) {
//// CurrentSpos = CurrentSpos.DOWN;
//// }
////
//// //set intake all teh way up when right bumper is pressed
//// if (controller2.getRightBumper().isJustPressed()) {
//// Currentpos = Currentpos.UP;
//// }
//
// // update the runtime on the telemetry
// telemetry.addData("Runtime", currentTime);
// telemetry.update();
// }
//}

99
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java Normal file
View File

@ -0,0 +1,99 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Intake;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
@Config
@TeleOp(name = "Main TeleOp", group = "OpModes")
public class NewTeleop extends OpMode {
public static double normal = 0.5;
public static double turbo = 1;
private Robot robot;
private Controller controller1;
private Controller controller2;
@Override
public void init() {
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
this.robot = new Robot(hardwareMap);
robot.intake.setpos(Intake.Position.STACK1);
telemetry.addLine("Started");
telemetry.update();
}
@Override
public void loop() {
// Driver 1
double x = -gamepad1.left_stick_y;
double y = -gamepad1.left_stick_x;
double z = -gamepad1.right_stick_x;
if (controller1.getA().isPressed()) {
x *= turbo;
y *= turbo;
z *= turbo;
} else {
x *= normal;
y *= normal;
z *= normal;
}
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// Driver 2
robot.intake.setDcMotor(gamepad2.right_trigger);
if (controller2.getRightBumper().isJustPressed()) {
robot.intake.incrementPos();
}
if (controller2.getLeftBumper().isJustPressed()) {
robot.intake.decrementPos();
}
// macros
switch (robot.runningMacro) {
case(0): // manual mode
robot.slides.increaseTarget(controller2.getLeftStick().getY());
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
} else if (controller2.getY().isJustPressed()) {
robot.runningMacro = 2;
} else if (controller2.getB().isJustPressed()) {
robot.runningMacro = 3;
} else if (controller2.getA().isJustPressed()) {
robot.runningMacro = 4;
}
break;
case(1):
robot.extendMacro(Slides.Position.TIER1, getRuntime());
break;
case(2):
robot.extendMacro(Slides.Position.TIER2, getRuntime());
break;
case(3):
robot.extendMacro(Slides.Position.TIER3, getRuntime());
break;
case(4):
robot.resetMacro(Slides.Position.DOWN, getRuntime());
break;
}
// update and telemetry
robot.update(getRuntime());
controller1.update();
controller2.update();
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java
View File

@ -43,9 +43,9 @@ public class DriveConstants {
* angular distances although most angular parameters are wrapped in Math.toRadians() for
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
*/
public static double WHEEL_RADIUS = 1.8045; // in
public static double WHEEL_RADIUS = 1.889765; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 15.75; // in
public static double TRACK_WIDTH = 13.5; // in
/*
* These are the feedforward parameters used to model the drive motor behavior. If you are using
@ -55,8 +55,8 @@ public class DriveConstants {
*/
// public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
public static double kV = 0.013;
public static double kA = 0.003;
public static double kV = 0.012;
public static double kA = 0.004;
public static double kStatic = 0;
/*
@ -68,10 +68,10 @@ public class DriveConstants {
*/
// old is 35 35 120 120
// new is 70 45 90 90
public static double MAX_VEL = 35;
public static double MAX_ACCEL = 35;
public static double MAX_ANG_VEL = Math.toRadians(90);
public static double MAX_ANG_ACCEL = Math.toRadians(90);
public static double MAX_VEL = 60;
public static double MAX_ACCEL = 60;
public static double MAX_ANG_VEL = Math.toRadians(120);
public static double MAX_ANG_ACCEL = Math.toRadians(120);
public static double encoderTicksToInches(double ticks) {

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java
View File

@ -39,7 +39,7 @@ import java.util.List;
@Config
public class SampleMecanumDrive extends MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(10, 0, 0);
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0);
public static double LATERAL_MULTIPLIER = 1;
@ -65,7 +65,7 @@ public class SampleMecanumDrive extends MecanumDrive {
super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH, DriveConstants.TRACK_WIDTH, LATERAL_MULTIPLIER);
follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
new Pose2d(0.5, 0.5, Math.toRadians(5)), 0.1);
new Pose2d(0.5, 0.5, Math.toRadians(5)), 0.5);
LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
@ -136,7 +136,7 @@ public class SampleMecanumDrive extends MecanumDrive {
// TODO: if desired, use setLocalizer() to change the localization method
// for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
// setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this));
setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this));
// setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap));
trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java
View File

@ -33,18 +33,18 @@ import java.util.List;
*
*/
public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
public static double TICKS_PER_REV = 8192;
public static double WHEEL_RADIUS = 0.6889764; // in
public static double TICKS_PER_REV = 2000;
public static double WHEEL_RADIUS = 0.944882; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double PARALLEL_X = -4.48818897638; // X is the up and down direction
public static double PARALLEL_Y = 3.10679133858; // Y is the strafe direction
public static double PARALLEL_X = -1.5; // X is the up and down direction
public static double PARALLEL_Y = 7.25; // Y is the strafe direction
public static double PERPENDICULAR_X = 2.21801181102;
public static double PERPENDICULAR_X = -6.1;
public static double PERPENDICULAR_Y = 0;
public static double MULTIPLIER_X = 1.4;
public static double MULTIPLIER_Y = 1.4;
public static double MULTIPLIER_X = 1.0;
public static double MULTIPLIER_Y = 1.0;
// Parallel/Perpendicular to the forward axis
// Parallel wheel is parallel to the forward axis
@ -61,12 +61,12 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
this.drive = drive;
// parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "parallel"));
parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backLeft"));
// parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight"));
// perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "perpendicular"));
perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "Intake Motor"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
// perpendicularEncoder.setDirection(Encoder.Direction.REVERSE);
perpendicularEncoder.setDirection(Encoder.Direction.REVERSE);
// parallelEncoder.setDirection(Encoder.Direction.REVERSE);
}