Skip to content

Commit

Permalink
NYC Qualifier #3
Browse files Browse the repository at this point in the history
pserb committed Dec 4, 2022
1 parent c81a0e2 commit 5747398
Showing 22 changed files with 275 additions and 76 deletions.
6 changes: 3 additions & 3 deletions drive/DriveConstants.java
Original file line number Diff line number Diff line change
@@ -34,7 +34,7 @@ public class DriveConstants {
* from DriveVelocityPIDTuner.
*/
public static final boolean RUN_USING_ENCODER = true;
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(40, 0, 16.5,
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(20, 0, 6,
13.5);
// public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(40, 0, 16.5,
// 13.1);
@@ -91,8 +91,8 @@ public class DriveConstants {
* You are free to raise this on your own if you would like. It is best determined through experimentation.
*/
public static double MAX_VEL = 70; // 47 for 5 cycles; 67 for 6 cycles
public static double MAX_ACCEL = 70; // 47 for 5 cycles; 67 for 6 cycles
public static double MAX_VEL = 30; // 47 for 5 cycles; 67 for 6 cycles
public static double MAX_ACCEL = 30; // 47 for 5 cycles; 67 for 6 cycles
public static double MAX_ANG_VEL = Math.toRadians(250);
public static double MAX_ANG_ACCEL = Math.toRadians(250);

4 changes: 2 additions & 2 deletions drive/SampleMecanumDrive.java
Original file line number Diff line number Diff line change
@@ -53,8 +53,8 @@
*/
@Config
public class SampleMecanumDrive extends MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(4, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(2, 0, 0);
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);

public static double LATERAL_MULTIPLIER = 1.5782;

6 changes: 4 additions & 2 deletions hardware/Arm.java
Original file line number Diff line number Diff line change
@@ -13,15 +13,17 @@ public class Arm extends Mechanism {
private Servo leftArm;
private Servo rightArm;

public static double INTAKE_POS = 0;
public static double SCORE_POS = 1;
public static double INTAKE_POS = 0.04;
public static double SCORE_POS = 0.72;

public Arm(LinearOpMode opMode) { this.opMode = opMode; }

@Override
public void init(HardwareMap hwMap) {
leftArm = hwMap.get(Servo.class, "leftArm");
rightArm = hwMap.get(Servo.class, "rightArm");

rightArm.setDirection(Servo.Direction.REVERSE);
}

public void intakePos() {
9 changes: 7 additions & 2 deletions hardware/Clamp.java
Original file line number Diff line number Diff line change
@@ -2,6 +2,7 @@

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@@ -11,15 +12,18 @@
public class Clamp extends Mechanism {

private Servo clamp;
// private CRServo crServo;

public static double OPEN_POS = 1;
public static double CLOSE_POS = 0;
public static double OPEN_POS = 0;
public static double CLOSE_POS = 1;

public Clamp(LinearOpMode opMode) { this.opMode = opMode; }

@Override
public void init(HardwareMap hwMap) {
clamp = hwMap.get(Servo.class, "clamp");
clamp.setDirection(Servo.Direction.REVERSE);

}

public void open() {
@@ -28,6 +32,7 @@ public void open() {

public void close() {
clamp.setPosition(CLOSE_POS);
// crServo.setPower(0);
}

@Override
9 changes: 9 additions & 0 deletions hardware/ConeSensor.java
Original file line number Diff line number Diff line change
@@ -5,6 +5,8 @@
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.stuyfission.fissionlib.util.Mechanism;

import org.firstinspires.ftc.robotcore.external.Telemetry;

public class ConeSensor extends Mechanism {

private ColorRangeSensor coneSensor;
@@ -37,4 +39,11 @@ public boolean hasCone() {
return hasConeColor(coneSensor);
}

@Override
public void telemetry(Telemetry telemetry) {
telemetry.addData("has cone?", hasCone());
telemetry.addData("blue value", getBlue(coneSensor));
telemetry.addData("red value", getRed(coneSensor));
}

}
4 changes: 2 additions & 2 deletions hardware/Intake.java
Original file line number Diff line number Diff line change
@@ -13,8 +13,8 @@ public class Intake extends Mechanism {
CRServo leftIntake;
CRServo rightIntake;

public static double LEFT_POWER = 1;
public static double RIGHT_POWER = -1;
public static double LEFT_POWER = -1;
public static double RIGHT_POWER = 1;

public Intake(LinearOpMode opMode) {
this.opMode = opMode;
2 changes: 1 addition & 1 deletion hardware/Robot.java
Original file line number Diff line number Diff line change
@@ -38,7 +38,7 @@ public void loop(Gamepad gamepad) {
@Override
public void telemetry(Telemetry telemetry) {
// mxSensor.telemetry(telemetry);
slidesFSM.telemetry(telemetry);
// slidesFSM.telemetry(telemetry);
scoringFSM.telemetry(telemetry);
}
}
20 changes: 15 additions & 5 deletions hardware/ScoringFSM.java
Original file line number Diff line number Diff line change
@@ -12,11 +12,11 @@
public class ScoringFSM extends Mechanism {

public static long ARM_TO_SCORE_DELAY = 200; // milliseconds
public static long ARM_TO_RESET_DELAY = 200; // milliseconds
public static long ARM_TO_RESET_DELAY = 500; // milliseconds

private Arm arm = new Arm(opMode);
private Clamp clamp = new Clamp(opMode);
private ConeSensor coneSensor = new ConeSensor(opMode);
// private ConeSensor coneSensor = new ConeSensor(opMode);

private Thread scoreReadyThread;
private Thread scoreResetThread;
@@ -27,7 +27,7 @@ public class ScoringFSM extends Mechanism {
public void init(HardwareMap hwMap) {
arm.init(hwMap);
clamp.init(hwMap);
coneSensor.init(hwMap);
// coneSensor.init(hwMap);

scoreReadyThread = new Thread(scoreReady);
scoreResetThread = new Thread(scoreReset);
@@ -52,7 +52,7 @@ public enum ScoreState {

public Runnable scoreReset = () -> {
try {
clamp.open();
// clamp.open();
Thread.sleep(ARM_TO_RESET_DELAY);
scoreState = ScoreState.REST;
} catch (InterruptedException e) {
@@ -69,11 +69,20 @@ public void loop(Gamepad gamepad) {
scoreState = ScoreState.WAIT_INTAKE;
break;
case WAIT_INTAKE:
if (coneSensor.hasCone()) {
if (gamepad.left_bumper) {
clamp.open();
} else if (gamepad.right_bumper) {
clamp.close();
} else if (gamepad.dpad_up) {
try {
scoreReadyThread.start();
} catch (IllegalThreadStateException ignored) {}
}
// if (coneSensor.hasCone()) {
// try {
// scoreReadyThread.start();
// } catch (IllegalThreadStateException ignored) {}
// }
break;
case SCORE:
clamp.close();
@@ -91,6 +100,7 @@ public void loop(Gamepad gamepad) {
@Override
public void telemetry(Telemetry telemetry) {
telemetry.addData("Current state", scoreState);
// coneSensor.telemetry(telemetry);
}

}
22 changes: 20 additions & 2 deletions hardware/SlidesFSM.java
Original file line number Diff line number Diff line change
@@ -12,12 +12,14 @@
public class SlidesFSM extends Mechanism {

public static long RETRACT_STATE_DELAY = 100;
public static long RETRACT_DELAY = 250;

private SlidesMotors slidesMotors = new SlidesMotors(opMode);

private Thread lowThread;
private Thread mediumThread;
private Thread highThread;
private Thread retractThread;

public SlidesFSM(LinearOpMode opMode) { this.opMode = opMode; }

@@ -28,6 +30,7 @@ public void init(HardwareMap hwMap) {
lowThread = new Thread(low);
mediumThread = new Thread(medium);
highThread = new Thread(high);
retractThread = new Thread(retract);
}

public enum SlidesState {
@@ -67,6 +70,16 @@ public enum SlidesState {
}
};

public Runnable retract = () -> {
try {
Thread.sleep(0);
slidesMotors.rest();
slidesState = SlidesState.REST;
} catch (InterruptedException e) {
e.printStackTrace();
}
};

@Override
public void loop(Gamepad gamepad) {
slidesMotors.update();
@@ -93,8 +106,13 @@ public void loop(Gamepad gamepad) {
}
break;
case WAIT_RETRACT:
if (gamepad.x) {
slidesState = SlidesState.REST;
if (gamepad.dpad_down) {
slidesMotors.descendABit();
}
else if (gamepad.x) {
try {
retractThread.start();
} catch (IllegalThreadStateException ignored) {}
}
break;
}
17 changes: 11 additions & 6 deletions hardware/SlidesMotors.java
Original file line number Diff line number Diff line change
@@ -20,18 +20,18 @@ public class SlidesMotors extends Mechanism {
private static final double GEAR_RATIO = 1.0;
private static final double TICKS_PER_REV = 145.1;

public static double MAX_VEL = 20;
public static double MAX_ACCEL = 20;
public static double RETRACTION_MULTIPLIER = 0.2;
public static double MAX_VEL = 50;
public static double MAX_ACCEL = 50;
public static double RETRACTION_MULTIPLIER = 0.5;

public static double kP = 0.1;
public static double kI = 0;
public static double kD = 0;
public static double kF = 0;

public static double POS_LOW = 1;
public static double POS_MEDIUM = 2;
public static double POS_HIGH = 3;
public static double POS_LOW = 33;
public static double POS_MEDIUM = 52;
public static double POS_HIGH = 65;

public SlidesMotors(LinearOpMode opMode) { this.opMode = opMode; }

@@ -76,6 +76,11 @@ public void extendHigh() {
rightSlideMotor.setTargetPosition(POS_HIGH);
}

public void descendABit() {
leftSlideMotor.setTargetPosition(leftSlideMotor.getPosition() - 3);
rightSlideMotor.setTargetPosition(rightSlideMotor.getPosition() - 3);
}

public void update() {
leftSlideMotor.update();
rightSlideMotor.update();
64 changes: 64 additions & 0 deletions opmode/auton/AllAuto.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
package org.firstinspires.ftc.teamcode.opmode.auton;

import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.Webcam;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;

@Autonomous (name = "ALL AUTO", group = "_main")
public class AllAuto extends LinearOpMode {

public static double FORWARD_DIST = 36;
public static double LATERAL_DIST = 23;

private SampleMecanumDrive drive;
private Webcam webcam = new Webcam(this);

@Override
public void runOpMode() throws InterruptedException {
drive = new SampleMecanumDrive(hardwareMap);
webcam.init(hardwareMap);

TrajectorySequence leftPark = drive.trajectorySequenceBuilder(new Pose2d())
.forward(FORWARD_DIST)
.waitSeconds(0.5)
.strafeLeft(LATERAL_DIST)
.build();

TrajectorySequence middlePark = drive.trajectorySequenceBuilder(new Pose2d())
.forward(FORWARD_DIST)
.build();

TrajectorySequence rightPark = drive.trajectorySequenceBuilder(new Pose2d())
.forward(FORWARD_DIST)
.waitSeconds(0.5)
.strafeRight(LATERAL_DIST)
.build();

waitForStart();

switch (webcam.side()) {
case ONE:
drive.followTrajectorySequenceAsync(leftPark);
break;
case TWO:
drive.followTrajectorySequenceAsync(middlePark);
break;
case THREE:
drive.followTrajectorySequenceAsync(rightPark);
break;
case NOT_FOUND:
default:
drive.followTrajectorySequenceAsync(middlePark);
break;
}

while (opModeIsActive() && !isStopRequested()) {
drive.update();
}

}
}
36 changes: 0 additions & 36 deletions opmode/auton/RedRight.java

This file was deleted.

Loading

0 comments on commit 5747398

Please sign in to comment.