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

started to erase the velocity code to start working on the autonomous with just power

Browse Source
This commit is contained in:
SpencerPiha
2024-02-08 20:31:35 -06:00
parent ebbbc9c263
commit 1339cc8ebf
4 changed files with 16 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -41,17 +41,17 @@ public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
// addTask(new ClawArmControlTask(robot));
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
// 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"));
// 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", "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 ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4"));

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

@@ -12,11 +12,11 @@ import dev.cyberarm.engine.V2.CyberarmState;
public class DriveToCoordinatesState extends CyberarmState {
CompetitionRobotV1 robot;
public double xTarget;
public double yTarget;
public double hTarget;
public double maxVelocityX;
public double maxVelocityY;
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 boolean armDrive;
@@ -69,7 +69,7 @@ public class DriveToCoordinatesState extends CyberarmState {
if (posSpecific) {
if (objectPos != robot.objectPos) {
// enter the ending loop
setHasFinished(true);
// setHasFinished(true);
} else {
if (armDrive) {
@@ -78,7 +78,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,7 +88,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);
}
}
}

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

@@ -112,7 +112,7 @@ public class CompetitionRobotV1 extends Robot {
public double xVelocity;
public double yVelocity;
public double deltaTime;
public double deltaTime = 0;
//-------------------------------------------------------------------------------------------------------------- arm sequence variables:
@@ -299,27 +299,6 @@ 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);
@@ -376,10 +355,6 @@ 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
targetVelocityX = XPIDControl(xTarget, positionX);
targetVelocityY = YPIDControl(yTarget, positionY);
XVeloPIDControl(targetVelocityX, xVelocity);
YVeloPIDControl(targetVelocityY, yVelocity);
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);

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

@@ -339,6 +339,7 @@ public class CompetitionTeleOpState extends CyberarmState {
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lastExecutedTime = System.currentTimeMillis();
}
@Override