Skip to content

Commit

Permalink
eddie tuning changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@powerfulHermes
powerfulHermes committed Feb 8, 2024
1 parent e214ba0 commit 2cca334
Show file tree
Hide file tree
Showing 5 changed files with 121 additions and 58 deletions.
31 changes: 22 additions & 9 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,27 +28,40 @@ public static double getMotorVelocityF(double ticksPerSecond) {
// see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
return 32767 / ticksPerSecond;
}
public static final double TICKS_PER_REV = 537.7;
public static final double MAX_RPM = 312;
// public static final double TICKS_PER_REV = 537.7;
// public static final double MAX_RPM = 312;
public static final double TICKS_PER_REV = 383.6;
public static final double MAX_RPM = 435;

public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
public static double GEAR_RATIO = 1;
public static double WHEEL_RADIUS = 1.8898;
public static double MAX_VEL = ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85;
// public static double MAX_VEL = ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85;
public static double MAX_VEL = 73.17330064499293;

public static double MAX_ACCEL = MAX_VEL;
public static double MAX_ANG_ACCEL = Math.toRadians(80.66924999999999);
public static double MAX_ANG_VEL = Math.toRadians(127.260357);
public static double TRACK_WIDTH = 15.96;
public static double kV = 0.014129716300132542;
public static double kA = 0.0032;
// public static double MAX_ANG_ACCEL = Math.toRadians(80.66924999999999);
// public static double MAX_ANG_VEL = Math.toRadians(127.260357);
public static double MAX_ANG_VEL = Math.toRadians(258.001310769);
public static double MAX_ANG_ACCEL = MAX_ANG_VEL;

// public static double TRACK_WIDTH = 15.96;
public static double TRACK_WIDTH = 16.25;

// public static double kV = 0.014129716300132542;
public static double kV = 0.01161656;

// public static double kA = 0.0032;
public static double kA = 0;
public static double kStatic = 0;
public static double DW_TICKS_PER_REV = 2048;
public static double DW_WHEEL_RADIUS = 0.944882; // in
public static double DW_GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed

public static double LATERAL_DISTANCE = 8; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = -6.5; // in; offset of the lateral wheel
//public static double FORWARD_OFFSET = -6.5; // in; offset of the lateral wheel
public static double FORWARD_OFFSET = -7.5;

private Encoder leftEncoder, rightEncoder, frontEncoder;
public static double X_MULTIPLIER = 88.6/90; // Multiplier in the X direction
Expand Down
41 changes: 23 additions & 18 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tuning/StrafeTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,43 +3,48 @@
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.Robot;
import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;

/*
* This is a simple routine to test translational drive capabilities.
*/
@Config
@Autonomous(group = "drive")
@Disabled
public class StrafeTest extends LinearOpMode {
public static double DISTANCE = 60; // in

@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());

Robot robot = Robot.thisRobot(hardwareMap);
//TestBotDrive drive = new TestBotDrive(hardwareMap);
TrajectoryDrive drive = robot.getDrive();

// Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
// .strafeRight(DISTANCE)
// .build();
//
// waitForStart();
//
// if (isStopRequested()) return;
//
// drive.followTrajectory(trajectory);
//
// Pose2d poseEstimate = drive.getPoseEstimate();
// telemetry.addData("finalX", poseEstimate.getX());
// telemetry.addData("finalY", poseEstimate.getY());
// telemetry.addData("finalHeading", poseEstimate.getHeading());
// telemetry.update();
//
// while (!isStopRequested() && opModeIsActive()) ;
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
.strafeRight(DISTANCE)
.build();

waitForStart();

if (isStopRequested()) return;

drive.followTrajectory(trajectory);

Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("finalX", poseEstimate.getX());
telemetry.addData("finalY", poseEstimate.getY());
telemetry.addData("finalHeading", poseEstimate.getHeading());
telemetry.update();

while (!isStopRequested() && opModeIsActive()) ;
}
}
45 changes: 25 additions & 20 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tuning/StraightTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,43 +3,48 @@
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.Robot;
import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;

/*
* This is a simple routine to test translational drive capabilities.
*/
@Config
@Autonomous(group = "drive")
@Disabled
public class StraightTest extends LinearOpMode {
public static double DISTANCE = 90; // in

@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());

// PsiDrive drive = new PsiDrive(hardwareMap);
//
// Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
// .forward(DISTANCE)
// .build();
//
// waitForStart();
//
// if (isStopRequested()) return;
//
// drive.followTrajectory(trajectory);
//
// Pose2d poseEstimate = drive.getPoseEstimate();
// telemetry.addData("finalX", poseEstimate.getX());
// telemetry.addData("finalY", poseEstimate.getY());
// telemetry.addData("finalHeading", poseEstimate.getHeading());
// telemetry.update();
//
// while (!isStopRequested() && opModeIsActive()) ;
Robot robot = Robot.thisRobot(hardwareMap);
TrajectoryDrive drive = robot.getDrive();
//PsiDrive drive = new PsiDrive(hardwareMap);

Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
.forward(DISTANCE)
.build();

waitForStart();

if (isStopRequested()) return;

drive.followTrajectory(trajectory);

Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("finalX", poseEstimate.getX());
telemetry.addData("finalY", poseEstimate.getY());
telemetry.addData("finalHeading", poseEstimate.getHeading());
telemetry.update();

while (!isStopRequested() && opModeIsActive()) ;
}
}
40 changes: 40 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/HWTEST.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
package org.firstinspires.ftc.teamcode.opmode;

import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.teamcode.drive.Robot;
import org.firstinspires.ftc.teamcode.drive.TrackingWheelLocalizer;
import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;


@TeleOp
// -34502 to -35218 -716
// -18503 to -19233 -730
// -14975 to -14200
public class HWTEST extends LinearOpMode {

@Override
public void runOpMode() throws InterruptedException {
// TestBotDrive drive = new TestBotDrive(hardwareMap);
Robot robot = Robot.thisRobot(hardwareMap);
TrajectoryDrive drive = robot.getDrive();
waitForStart();
while (opModeIsActive()) {
Vector2d input = new Vector2d(-gamepad1.left_stick_y, -gamepad1.left_stick_x);
drive.setWeightedDrivePower(new Pose2d(input.getX(), input.getY(), -gamepad1.right_stick_x));
drive.update();

android.util.Log.i("TESTODO", "LEFT " + ((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition());
android.util.Log.i("TESTODO", "RIGHT " + ((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition());
android.util.Log.i("TESTODO", "CENTER " + ((TrackingWheelLocalizer) drive.getLocalizer()).frontEncoder.getCurrentPosition());



}

}
}
22 changes: 11 additions & 11 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,9 @@
import org.firstinspires.ftc.teamcode.drive.Robot;
import org.firstinspires.ftc.teamcode.drive.RoboticaBot;
import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;
import org.firstinspires.ftc.teamcode.util.Debouncer;
import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
import org.firstinspires.ftc.teamcode.vision.AprilTagLocalizer;

@TeleOp
public class TestTeleOp extends LinearOpMode {
Expand Down Expand Up @@ -78,22 +80,20 @@ public void runOpMode() throws InterruptedException {

if (robot.getClass() == PsiBot.class) {



if (gamepad1.left_bumper) {
if (gamepad1.left_bumper || gamepad2.left_bumper) {
purplePose = purplePose + 0.1;
} else if (gamepad1.right_bumper) {
} else if (gamepad1.right_bumper|| gamepad2.right_bumper) {
purplePose = purplePose - 0.1;
}
if (purplePose > 1) purplePose = 1;
if (purplePose < 0) purplePose = 0;
((PsiBot) robot).planeServo.setPosition(purplePose);
telemetry.addData("Plane Pose", purplePose);
((PsiBot) robot).armMotor.setPower((gamepad1.left_trigger*0.1)-((gamepad1.right_trigger*0.1)));
telemetry.addData("Arm location", ((PsiBot) robot).armMotor.getCurrentPosition());
if (gamepad1.b) {
// telemetry.addData("Plane Pose", purplePose);
((PsiBot) robot).armMotor.setPower((gamepad1.left_trigger*0.1)-((gamepad1.right_trigger*0.1) + (gamepad2.left_trigger*0.1)-((gamepad2.right_trigger*0.1))));
// telemetry.addData("Arm location", ((PsiBot) robot).armMotor.getCurrentPosition());
if (gamepad1.b || gamepad2.b) {
((PsiBot) robot).mosaicServo.setPosition(0);
} else if (gamepad1.a) {
} else if (gamepad1.a || gamepad2.a) {
((PsiBot) robot).mosaicServo.setPosition(1);
}
android.util.Log.i("TELEOP", "LOOP2A");
Expand Down Expand Up @@ -161,8 +161,8 @@ public void runOpMode() throws InterruptedException {
}
//telemetry.addData("Wrist Pose", wristPose);

//telemetry.addData("Arm Pose", rrobot.armMotor.getCurrentPosition());
//telemetry.addData("Extend Amount", rrobot.intakeServo.getAdjustedPosition());
// telemetry.addData("Arm Pose", rrobot.armMotor.getCurrentPosition());
// telemetry.addData("Extend Amount", rrobot.intakeServo.getAdjustedPosition());

}
android.util.Log.i("TELEOP", "LOOPEND");
Expand Down

0 comments on commit 2cca334

Please sign in to comment.