TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-12 23:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

tuning auto today

Browse Source
This commit is contained in:
SpencerPiha
2024-02-10 14:59:07 -06:00
parent 1339cc8ebf
commit b8814b1084
5 changed files with 119 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropRed.java Normal file
View File

@@ -0,0 +1,92 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "State BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
public class StateBackDropRed extends CyberarmEngine {
CompetitionRobotV1 robot;
@Override
public void init() {
super.init();
robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.clawArm.setTargetPosition(0);
robot.clawArm.setPower(0);
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.imu.resetYaw();
robot.leftClaw.setPosition(0.25);
robot.rightClaw.setPosition(0.6);
}
@Override
public void setup() {
this.robot = new CompetitionRobotV1("State BackDrop red");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
this.robot.setup();
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"State BackDrop red", "1-00-0"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-03-0"));
// bring arm to hover
addState(new ClawArmState(robot,"State BackDrop red", "3-00-0"));
//open claw
addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0"));
addState(new ClawArmState(robot,"State BackDrop red", "5-00-0"));
// drive towards backboard
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-1"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-0"));
// drive into board
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-2"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-1"));
//open right close left
addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0"));
// bring arm up
addState(new ClawArmState(robot,"State BackDrop red", "8-00-0"));
}
}

7
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java
View File

@@ -54,7 +54,6 @@ public class CameraVisionState extends CyberarmState {
@Override
public void exec() {
robot.clawArmControl();
if (System.currentTimeMillis() - initTime > 4000){
robot.objectPos = pipeline.objectPos();
setHasFinished(true);
@@ -84,6 +83,7 @@ public class CameraVisionState extends CyberarmState {
Mat leftCrop;
Mat middleCrop;
Mat rightCrop;
Mat rotatedMat = new Mat();
double leftavgfin;
double middleavgfin;
double rightavgfin;
@@ -91,13 +91,14 @@ public class CameraVisionState extends CyberarmState {
Scalar rectColor = new Scalar(255.0, 0.0, 0.0);
public Mat processFrame(Mat input) {
Imgproc.cvtColor(input, HSV, Imgproc.COLOR_RGB2HSV);
Core.rotate(input, rotatedMat,Core.ROTATE_180);
Imgproc.cvtColor(rotatedMat, HSV, Imgproc.COLOR_RGB2HSV);
Rect leftRect = new Rect(1, 1, 212, 359);
Rect rightRect = new Rect(213, 1, 212, 359);
Rect middleRect = new Rect(426, 1, 212, 359);
input.copyTo(output);
rotatedMat.copyTo(output);
Imgproc.rectangle(output, leftRect, rectColor, 2);
Imgproc.rectangle(output, rightRect, rectColor, 2);
Imgproc.rectangle(output, middleRect, rectColor, 2);

32
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
View File

@@ -12,13 +12,9 @@ import dev.cyberarm.engine.V2.CyberarmState;
public class DriveToCoordinatesState extends CyberarmState {
CompetitionRobotV1 robot;
public static double xTarget;
public static double yTarget;
public static double hTarget;
public static double maxVelocityX;
public static double maxVelocityY;
public double maxVelocityH;
public boolean posAchieved = false;
public double xTarget;
public double yTarget;
public double hTarget;
public boolean armDrive;
public int objectPos;
public boolean posSpecific;
@@ -43,7 +39,7 @@ public class DriveToCoordinatesState extends CyberarmState {
@Override
public void start() {
super.start();
if (posSpecific) {
if (posSpecific && objectPos == robot.objectPos) {
robot.hTarget = hTarget;
robot.yTarget = yTarget;
robot.xTarget = xTarget;
@@ -60,16 +56,16 @@ public class DriveToCoordinatesState extends CyberarmState {
@Override
public void exec() {
if (robot.xVelocity > maxVelocityX){
maxVelocityX = robot.xVelocity;
}
if (robot.yVelocity > maxVelocityY){
maxVelocityY = robot.yVelocity;
}
// robot.yMaxPower = maxYPower;
// robot.xMaxPower = maxXPower;
// robot.hTarget = hTarget;
// robot.yTarget = yTarget;
// robot.xTarget = xTarget;
if (posSpecific) {
if (objectPos != robot.objectPos) {
// enter the ending loop
// setHasFinished(true);
setHasFinished(true);
} else {
if (armDrive) {
@@ -78,7 +74,7 @@ public class DriveToCoordinatesState extends CyberarmState {
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
// setHasFinished(true);
setHasFinished(true);
}
}
} else {
@@ -88,15 +84,13 @@ public class DriveToCoordinatesState extends CyberarmState {
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
// setHasFinished(true);
setHasFinished(true);
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("x velocity max", maxVelocityX);
engine.telemetry.addData("y velocity max", maxVelocityY);
engine.telemetry.addData("x pos", robot.positionX);
engine.telemetry.addData("y pos", robot.positionY);
engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH));

18
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
View File

@@ -51,10 +51,8 @@ public class CompetitionRobotV1 extends Robot {
// ----------------------------------------------------------------------------------------------------------------- odometry variables:
public static double Hp = 0.8, Hi = 0, Hd = 0;
public static double Xp = -0.03, Xi = 0, Xd = 0;
public static double xvp = -0, xvi = 0, xvd = 0;
public static double Yp = 0.03, Yi = 0, Yd = 0;
public static double yvp = 0.03, yvi = 0, yvd = 0;
public static double Xp = 0.03, Xi = 0, Xd = 0;
public static double Yp = -0.03, Yi = 0, Yd = 0;
public double Dnl1;
public double Dnr2;
@@ -76,7 +74,7 @@ public class CompetitionRobotV1 extends Robot {
public int oldLeftPosition = 0;
public int oldAuxPosition = 0;
public final static double L = 20.9; // distance between left and right encoder in cm
final static double B = 15; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
final static double B = 12.6; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
public final static double R = 3; // wheel radius in cm
final static double N = 8192; // encoder ticks per revolution (REV encoder)
@@ -118,14 +116,14 @@ public class CompetitionRobotV1 extends Robot {
//-------------------------------------------------------------------------------------------------------------- arm sequence variables:
PIDController pidController;
public double power;
public String armPos;
public String armPos = "hover";
public long armTime;
public int target;
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public double shoulderCollect = 0.86;
public double shoulderDeposit = 0.86;
public double shoulderDeposit = 1;
public double shoulderPassive = 1;
public double elbowCollect = 0.02;
public double elbowDeposit = 0;
@@ -393,7 +391,7 @@ public class CompetitionRobotV1 extends Robot {
if (Objects.equals(armPos, "deposit")) {
shoulder.setPosition(shoulderDeposit);
elbow.setPosition(elbowDeposit);
target = 400;
target = 300;
}
@@ -407,8 +405,8 @@ public class CompetitionRobotV1 extends Robot {
}
if (armPos.equals("search")) {
shoulder.setPosition(0.48);
if (armTime > 400){
shoulder.setPosition(0.55);
if (armTime > 450){
target = 570;
}

4
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java
View File

@@ -34,8 +34,8 @@ public class CompetitionTeleOpState extends CyberarmState {
// ------------------------------------------------------------------------------------------------------------- Heading lock variables:
public double integralSum = 0;
private double targetHeading;
public double collectLock = Math.toRadians(90);
public double backDropLock = Math.toRadians(-90);
public double collectLock = Math.toRadians(-90);
public double backDropLock = Math.toRadians(90);
public double boost;
public double armPower;
private double currentHeading;