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

wrote the implementation for tuning velocity, I plan to tune in the teleOp and then copy over to autonomous

Browse Source
This commit is contained in:
SpencerPiha
2024-02-06 23:47:27 -06:00
parent 4450c7e48a
commit ebbbc9c263
3 changed files with 30 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -15,6 +15,9 @@ public class DriveToCoordinatesState extends CyberarmState {
public double xTarget;
public double yTarget;
public double hTarget;
public double maxVelocityX;
public double maxVelocityY;
public double maxVelocityH;
public boolean posAchieved = false;
public boolean armDrive;
public int objectPos;
@@ -57,6 +60,12 @@ public class DriveToCoordinatesState extends CyberarmState {
@Override
public void exec() {
if (robot.xVelocity > maxVelocityX){
maxVelocityX = robot.xVelocity;
}
if (robot.yVelocity > maxVelocityY){
maxVelocityY = robot.yVelocity;
}
if (posSpecific) {
if (objectPos != robot.objectPos) {
// enter the ending loop
@@ -86,6 +95,8 @@ public class DriveToCoordinatesState extends CyberarmState {
@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));

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

@@ -52,7 +52,7 @@ 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 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;
@@ -75,7 +75,7 @@ public class CompetitionRobotV1 extends Robot {
public int oldRightPosition = 0;
public int oldLeftPosition = 0;
public int oldAuxPosition = 0;
public final static double L = 22.5; // distance between left and right encoder in cm
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
public final static double R = 3; // wheel radius in cm
final static double N = 8192; // encoder ticks per revolution (REV encoder)
@@ -376,6 +376,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
targetVelocityX = XPIDControl(xTarget, positionX);
targetVelocityY = YPIDControl(yTarget, positionY);
XVeloPIDControl(targetVelocityX, xVelocity);
YVeloPIDControl(targetVelocityY, yVelocity);

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

@@ -28,6 +28,9 @@ public class CompetitionTeleOpState extends CyberarmState {
public static double holdPos = 0.55 ;
public double maxVelocityX;
public double maxVelocityY;
// ------------------------------------------------------------------------------------------------------------- Heading lock variables:
public double integralSum = 0;
private double targetHeading;
@@ -110,7 +113,6 @@ public class CompetitionTeleOpState extends CyberarmState {
}
}
public void DriveTrainTeleOp () {
boolean lbs1 = engine.gamepad1.left_stick_button;
@@ -340,7 +342,13 @@ public class CompetitionTeleOpState extends CyberarmState {
}
@Override
public void exec() {
public void exec() { //------------------------------------------------------------------------------------------------------ EXEC Start
if (robot.xVelocity > maxVelocityX){
maxVelocityX = robot.xVelocity;
}
if (robot.yVelocity > maxVelocityY){
maxVelocityY = robot.yVelocity;
}
robot.OdometryLocalizer();
if (engine.gamepad2.start && engine.gamepad2.x){
@@ -409,10 +417,15 @@ public class CompetitionTeleOpState extends CyberarmState {
ClawControlTeleOp();
robot.velocityChecker();
}
@Override
public void telemetry () {
engine.telemetry.addData("x velocity max", maxVelocityX);
engine.telemetry.addData("y velocity max", maxVelocityY);
engine.telemetry.addData("Dnl1", robot.Dnl1);
engine.telemetry.addData("Dnr2", robot.Dnr2);
engine.telemetry.addData("x pos", robot.positionX);