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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
NerdyBirdy460
2024-01-20 12:07:03 -06:00
parent 2216ae2136 f47ca4c8f2
commit 513045b543
8 changed files with 84 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java
View File

@@ -3,6 +3,7 @@ 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.ClawArmControlTask;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
@@ -13,7 +14,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville audience blue")
@Autonomous(name = "Burnsville audience blue", preselectTeleOp = "Competition V1 TeleOp")
public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine {
@@ -42,15 +43,15 @@ public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine {
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-0"));
// addState(new CameraVisionState(robot));
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-1"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-1"));
addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-02-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-0"));
// place pixel
addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-1"));

9
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java
View File

@@ -3,6 +3,7 @@ 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;
@@ -12,7 +13,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville audience red")
@Autonomous(name = "Burnsville audience red", preselectTeleOp = "Competition V1 TeleOp")
public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine {
@@ -41,15 +42,15 @@ public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine {
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville audience red", "0-01-0"));
// addState(new CameraVisionState(robot));
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"Burnsville audience red", "0-01-1"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "2-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "1-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "1-02-0"));
// place pixel
addState(new ClawFingerState(robot,"Burnsville audience red", "0-02-1"));

9
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java
View File

@@ -3,6 +3,7 @@ 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;
@@ -13,7 +14,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville BackDrop blue")
@Autonomous(name = "Burnsville BackDrop blue", preselectTeleOp = "Competition V1 TeleOp")
public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
@@ -42,15 +43,15 @@ public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
// addState(new CameraVisionState(robot));
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4"));

17
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java
View File

@@ -3,6 +3,7 @@ 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;
@@ -12,7 +13,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville BackDrop red")
@Autonomous(name = "Burnsville BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {
@@ -22,7 +23,6 @@ public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {
@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);
@@ -36,19 +36,18 @@ public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {
this.robot = new CompetitionRobotV1("Burnsville BackDrop red");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
// addTask(new ClawArmControlTask(robot));
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-0"));
// addState(new CameraVisionState(robot));
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-1"));
// drive to the left, center, or right spike mark
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-1"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-02-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-02-5"));
@@ -67,8 +66,8 @@ public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-03-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-04-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-04-1"));

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

@@ -55,7 +55,7 @@ public class CameraVisionState extends CyberarmState {
@Override
public void exec() {
robot.clawArmControl();
if (System.currentTimeMillis() - initTime > 3000){
if (System.currentTimeMillis() - initTime > 4000){
robot.objectPos = pipeline.objectPos();
setHasFinished(true);
}

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

@@ -40,11 +40,15 @@ public class DriveToCoordinatesState extends CyberarmState {
@Override
public void start() {
super.start();
robot.hTarget = hTarget;
robot.yTarget = yTarget;
robot.xTarget = xTarget;
robot.yMaxPower = maxYPower;
robot.xMaxPower = maxXPower;
if (posSpecific) {
robot.hTarget = hTarget;
robot.yTarget = yTarget;
robot.xTarget = xTarget;
robot.yMaxPower = maxYPower;
robot.xMaxPower = maxXPower;
} else {
setHasFinished(true);
}
Log.d("TTT?", "" + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
Log.d("TTT?", "" + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget);

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

@@ -17,6 +17,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.openftc.easyopencv.OpenCvCameraRotation;
@@ -51,7 +52,9 @@ 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.03, 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 double xMultiplier = 1;
public double yMultiplier = 1;
public double positionX = 1000;
@@ -60,6 +63,8 @@ public class CompetitionRobotV1 extends Robot {
public double xTarget = 1000;
public double yTarget = 1000;
public double hTarget = 0;
public double targetVelocityX = 0;
public double targetVelocityY = 0;
public int currentRightPosition = 0;
public int currentLeftPosition = 0;
@@ -75,14 +80,20 @@ public class CompetitionRobotV1 extends Robot {
// heading math variables for pid with imu
public double headingIntegralSum = 0;
public double xIntegralSum = 0;
public double xVeloIntegralSum = 0;
public double yIntegralSum = 0;
public double yVeloIntegralSum = 0;
public final double cm_per_tick = (2 * Math.PI * R) / N;
private double headingLastError = 0;
private double xLastError = 0;
private double xVeloLastError = 0;
private double yLastError = 0;
private double yVeloLastError = 0;
ElapsedTime headingTimer = new ElapsedTime();
ElapsedTime xTimer = new ElapsedTime();
ElapsedTime yTimer = new ElapsedTime();
ElapsedTime xVeloTimer = new ElapsedTime();
ElapsedTime yVeloTimer = new ElapsedTime();
public double frontLeftPower;
public double backLeftPower;
@@ -96,6 +107,11 @@ public class CompetitionRobotV1 extends Robot {
public double rawPidX;
public double rawPidY;
public double xVelocity;
public double yVelocity;
public double deltaTime;
//-------------------------------------------------------------------------------------------------------------- arm sequence variables:
PIDController pidController;
public double power;
@@ -211,6 +227,17 @@ public class CompetitionRobotV1 extends Robot {
// -------------------------------------------------------------------------------------------------------------------------- Functions:
public void velocityChecker(){
long startTime = System.currentTimeMillis();
double oldXpos = positionX;
double oldYpos = positionX;
xVelocity = (oldXpos - positionX) / deltaTime;
yVelocity = (oldYpos - positionY) / deltaTime;
deltaTime = startTime - System.currentTimeMillis();
}
public void OdometryLocalizer() { // ------------------------------------------------------------------------------- Odometry Localizer:
// update positions
@@ -266,6 +293,27 @@ public class CompetitionRobotV1 extends Robot {
double output = (error * Hp) + (derivative * Hd) + (headingIntegralSum * Hi);
return output;
}
public double XVeloPIDControl ( double reference, double current){
double error = (reference - current);
xVeloIntegralSum += error * xVeloTimer.seconds();
double derivative = (error - xVeloLastError) / xVeloTimer.seconds();
xTimer.reset();
double output = (error * xvp) + (derivative * xvd) + (xIntegralSum * xvi);
return output;
}
public double YVeloPIDControl ( double reference, double current){
double error = (reference - current);
yVeloIntegralSum += error * yVeloTimer.seconds();
double derivative = (error - yVeloLastError) / xTimer.seconds();
xTimer.reset();
double output = (error * yvp) + (derivative * yvd) + (yVeloIntegralSum * yvi);
return output;
}
public double XPIDControl ( double reference, double current){
double error = (reference - current);
@@ -322,6 +370,8 @@ public class CompetitionRobotV1 extends Robot {
public void DriveToCoordinates () {
// determine the powers needed for each direction
// this uses PID to adjust needed Power for robot to move to target
XVeloPIDControl(targetVelocityX, xVelocity);
XVeloPIDControl(targetVelocityY, yVelocity);
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);

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

@@ -257,16 +257,6 @@ public class CompetitionTeleOpState extends CyberarmState {
}
}
if (Objects.equals(armPos, "passive")) {
if (robot.lift.getCurrentPosition() >= 20) {
robot.chinUpServo.setPosition(chinUpServoDown);
robot.lift.setPower(-0.6);
} else {
robot.shoulder.setPosition(robot.shoulderPassive);
target = 570;
}
}
if (Objects.equals(armPos, "lift up")) {
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowDeposit);
@@ -303,8 +293,6 @@ public class CompetitionTeleOpState extends CyberarmState {
super.init();
pidController = new PIDController(p, i, d);
robot.imu.resetYaw();
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);