TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 08:42:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

fixed heading lock, started to fix the max power function from the drive to coordinates state. plan to finish tomorrow and re tune the PID for driving because the robot has gotten changed mechanically, so it drives too fast

Browse Source
This commit is contained in:
SpencerPiha
2024-01-03 23:00:04 -06:00
parent 28933fc5c8
commit c5d5ca7b59
3 changed files with 56 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -48,11 +48,11 @@ public class DriveToCoordinatesState extends CyberarmState {
} else {
if (objectPos != robot.objectPos) {
// enter the ending loop
setHasFinished(true);
// setHasFinished(true);
}
}
if (posAchieved) {
setHasFinished(true);
// setHasFinished(true);
}
if (armDrive) {
@@ -93,6 +93,10 @@ public class DriveToCoordinatesState extends CyberarmState {
engine.telemetry.addData("front left power", robot.frontLeftPower);
engine.telemetry.addData("back right power", robot.backRightPower);
engine.telemetry.addData("back left power", robot.backLeftPower);
engine.telemetry.addData("input y pidPower", robot.pidX);
engine.telemetry.addData("input x pidPower", robot.pidY);
engine.telemetry.addData("raw x pid", robot.XPIDControl(robot.xTarget, robot.positionX));
engine.telemetry.addData("raw y pid", robot.YPIDControl(robot.yTarget, robot.positionY));
engine.telemetry.addData("global object position", robot.objectPos);
engine.telemetry.addData("local object position", objectPos);

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

@@ -36,7 +36,6 @@ public class CompetitionRobotV1 extends Robot {
public int objectPos;
// ------------------------------------------------------------------------------------------------------------------ HardwareMap setup:
public double power;
public OpenCvWebcam webcam1 = null;
public WebcamName webCamName;
public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp;
@@ -45,13 +44,11 @@ public class CompetitionRobotV1 extends Robot {
public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo;
public DistanceSensor customObject;
public TouchSensor touchLeftArm, touchRightArm;
public int loopCheck = 0;
// ----------------------------------------------------------------------------------------------------------------- odometry variables:
public static double Hp = 0.8, Hi = 0, Hd = 0;
public static double Xp = -0.035, Xi = 0, Xd = 0;
public static double Yp = 0.035, Yi = 0, Yd = 0.0013;
public double rx;
public double xMultiplier = 1;
public double yMultiplier = 1;
public double positionX = 1000;
@@ -93,10 +90,12 @@ public class CompetitionRobotV1 extends Robot {
public double xMaxPower = 1;
public double pidX;
public double pidY;
public double rawPidX;
public double rawPidY;
//-------------------------------------------------------------------------------------------------------------- arm sequence variables:
PIDController pidController;
public double power;
public String armPos;
public int target;
@@ -106,7 +105,6 @@ public class CompetitionRobotV1 extends Robot {
public double shoulderPassive = 0.8;
public double elbowCollect = 0.02;
public double elbowDeposit = 0;
public double elbowPassive = 0;
private HardwareMap hardwareMap;
@@ -288,26 +286,42 @@ public class CompetitionRobotV1 extends Robot {
}
public void DriveToCoordinates () {
// determine the velocities needed for each direction
// determine the powers needed for each direction
// this uses PID to adjust needed Power for robot to move to target
// if (yMaxPower < XPIDControl(xTarget, positionX)){
// pidX = yMaxPower;
// } else {
// pidX = YPIDControl(yTarget, positionY);
// }
// rawPidY = XPIDControl(xTarget, positionX);
// rawPidX = YPIDControl(yTarget, positionY);
if (Math.abs(yTarget - positionY) > 5) {
if (Math.abs(XPIDControl(xTarget, positionX)) >= yMaxPower) {
if (XPIDControl(xTarget, positionX) < 0) {
pidY = yMaxPower * -1;
} else {
pidY = yMaxPower;
}
} else {
pidY = rawPidX;
}
} else {
pidY = rawPidX;
}
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
// if (xMaxPower < YPIDControl(yTarget, positionY)){
// pidY = xMaxPower;
// } else {
// pidY = XPIDControl(xTarget, positionX);
// }
pidY = XPIDControl(xTarget, positionX);
pidX = YPIDControl(yTarget, positionY);
if (Math.abs(xTarget - positionX) > 5) {
if (Math.abs(YPIDControl(yTarget, positionY)) >= xMaxPower) {
if (YPIDControl(yTarget, positionY) < 0) {
pidX = xMaxPower * -1;
} else {
pidX = xMaxPower;
}
} else {
pidX = rawPidY;
}
} else {
pidX = rawPidY;
}
double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);

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

@@ -20,8 +20,8 @@ public class CompetitionTeleOpState extends CyberarmState {
private CompetitionRobotV1 robot;
// ------------------------------------------------------------------------------------------------------------robot claw arm variables:
private PIDController pidController;
public static double p = 0.007, i = 0, d = 0.0001, f = 0;
public static int target = 0;
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public int target = 0;
// ------------------------------------------------------------------------------------------------------------- Heading lock variables:
public double integralSum = 0;
@@ -29,7 +29,6 @@ public class CompetitionTeleOpState extends CyberarmState {
public double collectLock = Math.toRadians(-90);
public double backDropLock = Math.toRadians(90);
public double power;
public double armPower;
private double currentHeading;
private boolean headingLock = false;
@@ -107,12 +106,6 @@ public class CompetitionTeleOpState extends CyberarmState {
public void DriveTrainTeleOp () {
if (headingLock){
rx = HeadingPIDControl(targetHeading, currentHeading);
} else {
rx = engine.gamepad1.right_stick_x / 2;
}
boolean lbs1 = engine.gamepad1.left_stick_button;
if (lbs1 && !lbsVar1) {
@@ -239,18 +232,6 @@ public class CompetitionTeleOpState extends CyberarmState {
}
}
// if (armPos == "passive") {
// if (robot.lift.getCurrentPosition() >= 20) {
// robot.lift.setPower(-0.6);
// robot.chinUpServo.setPosition(chinUpServoDown);
// } else {
// robot.lift.setPower(0);
// robot.chinUpServo.setPosition(chinUpServoDown);
// robot.shoulder.setPosition(robot.shoulderPassive);
// robot.elbow.setPosition(robot.elbowPassive);
// target = 850;
// }
// }
if (Objects.equals(armPos, "deposit")) {
robot.shoulder.setPosition(robot.shoulderDeposit);
robot.elbow.setPosition(robot.elbowDeposit);
@@ -306,12 +287,12 @@ public class CompetitionTeleOpState extends CyberarmState {
super.init();
pidController = new PIDController(p, i, d);
}
@Override
public void exec() {
if (engine.gamepad2.dpad_up) {
robot.chinUp.setPower(-1);
} else if (engine.gamepad2.dpad_down){
@@ -327,24 +308,28 @@ public class CompetitionTeleOpState extends CyberarmState {
currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
// drivetrain
DriveTrainTeleOp();
if (engine.gamepad1.b){
headingLock = true;
targetHeading = backDropLock;
}
if (engine.gamepad1.x){
} else if (engine.gamepad1.x){
headingLock = true;
targetHeading = collectLock;
}
if (engine.gamepad1.a){
} else if (engine.gamepad1.a){
headingLock = true;
targetHeading = currentHeading;
}
if (engine.gamepad1.right_stick_x != 0){
} else if (engine.gamepad1.right_stick_x != 0){
headingLock = false;
}
if (headingLock){
rx = HeadingPIDControl(targetHeading, currentHeading);
} else {
rx = engine.gamepad1.right_stick_x / 2;
}
DriveTrainTeleOp();
// ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift:
pidController.setPID(p, i, d);
int armCurrentPos = robot.clawArm.getCurrentPosition();
@@ -375,8 +360,8 @@ public class CompetitionTeleOpState extends CyberarmState {
engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
engine.telemetry.addData("pid power", power);
engine.telemetry.addData("rx power", robot.rx);
engine.telemetry.addData("pid power", HeadingPIDControl(targetHeading, currentHeading));
engine.telemetry.addData("rx power", rx);
engine.telemetry.addData("heading Lock?", headingLock);
engine.telemetry.addData("Kp", Kp);
engine.telemetry.addData("Ki", Ki);