SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

merge into: SCDS:branch-rc-chassis-14493-subsystem-actions
Branches Tags
SCDS:master SCDS:branch-swiss-cheese SCDS:branch-rc-chassis-14493-subsystem-actions SCDS:branch-silver-14493-v2 SCDS:branch-rc-chassis-14493-new-subsystem-actions SCDS:branch-silver-14493 SCDS:branch-rc-chassis-14493-subsystem SCDS:branch-rc-chassis-14493 SCDS:branch-bobito-14493 SCDS:branch-black-14493 SCDS:branch-pedro-led-light SCDS:branch-asher SCDS:branch-pedro-into-the-deep SCDS:branch-alex SCDS:branch-carlos SCDS:branch-into-the-deep
...
pull from: SCDS:branch-bobito-14493
Branches Tags
SCDS:branch-swiss-cheese SCDS:branch-rc-chassis-14493-subsystem-actions SCDS:branch-silver-14493-v2 SCDS:branch-rc-chassis-14493-new-subsystem-actions SCDS:branch-silver-14493 SCDS:branch-rc-chassis-14493-subsystem SCDS:branch-rc-chassis-14493 SCDS:branch-bobito-14493 SCDS:branch-black-14493 SCDS:branch-pedro-led-light SCDS:branch-asher SCDS:branch-pedro-into-the-deep SCDS:branch-alex SCDS:branch-carlos SCDS:branch-into-the-deep SCDS:master

7 Commits
branch-rc- ... branch-bob

Author SHA1 Message Date
Carlos Rivas
bee78d7d71 WIP bobito tunings 2024-10-30 19:39:49 -07:00
Carlos Rivas
b442008fb6 Updating localizer/tuning values for Bobito 2024-10-30 12:54:51 -07:00
Carlos Rivas
4e8c37e01b Added motor direction logic 2024-10-22 22:46:20 -07:00
Carlos Rivas
6a125ff247 Added tuned values for Bobito 2024-10-22 22:46:04 -07:00
Carlos Rivas
1c4b3723c3 Added debugging for encoder logic 2024-10-22 22:44:55 -07:00
Carlos Rivas
7a028835f8 Removing unnecessary files for the current iteration of this drive encoded robot 2024-10-22 22:44:24 -07:00
Carlos Rivas
cd10d3a01b Preliminary configuration for Bobito as a DriveEncoder robot 2024-10-22 20:36:02 -07:00
10 changed files with 104 additions and 1193 deletions
Expand all files Collapse all files

73
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java
View File

@ -29,8 +29,6 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
@ -39,12 +37,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
@ -81,12 +74,15 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*/
@TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode")
@TeleOp(name = "Basic: Omni Linear OpMode", group = "Linear OpMode")
public class BasicOmniOpMode_Linear extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
private Encoder leftFront;
private Encoder rightFront;
private Encoder leftRear;
private Encoder rightRear;
@Override
public void runOpMode() {
@ -97,18 +93,12 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR));
leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR));
rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR));
rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR));
// TODO: replace these with your encoder ports
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
Encoder rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
Encoder strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
// ########################################################################################
// !!! IMPORTANT Drive Information. Test your motor directions. !!!!!
// ########################################################################################
@ -124,11 +114,20 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
leftBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Wait for the game to start (driver presses START)
telemetry.addData("Status", "Initialized");
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
telemetry.update();
waitForStart();
@ -139,16 +138,16 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
double max;
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;
// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
double leftFrontPower = axial + lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
@ -157,10 +156,10 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
max = Math.max(max, Math.abs(rightBackPower));
if (max > 1.0) {
leftFrontPower /= max;
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
leftBackPower /= max;
rightBackPower /= max;
}
// This is test code:
@ -187,12 +186,14 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
rightBackDrive.setPower(rightBackPower);
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Status", "Run Time: " + runtime);
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
telemetry.addData("Encoder Front Left", leftFrontDrive.getDirection() + " : " + leftFrontDrive.getCurrentPosition());
telemetry.addData("Encoder Front Right", rightFrontDrive.getCurrentPosition());
telemetry.addData("Encoder Back Left", leftBackDrive.getCurrentPosition());
telemetry.addData("Encoder Back Right", rightBackDrive.getCurrentPosition());
telemetry.update();
}
}}
}
}

41
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java
View File

@ -10,8 +10,6 @@ public class PedroConstants {
/*
Robot parameters
*/
// Turn localizer - -0.003
// Robot motor configurations
public static final String FRONT_LEFT_MOTOR = "Drive front lt";
@ -22,51 +20,38 @@ public class PedroConstants {
// Robot motor direction
public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
public static final Direction BACK_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
// Robot IMU configuration
public static final String IMU = "imu";
// Robot IMU placement
public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
= RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
= RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
// Robot encoders
public static final String LEFT_ENCODER = "encoder left";
public static final String RIGHT_ENCODER = "encoder right";
public static final String BACK_ENCODER = "encoder back";
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.REVERSE;
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.REVERSE;
// Robot encoder direction
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double FRONT_LEFT_MOTOR_ENCODER = Encoder.FORWARD;
public static final double BACK_LEFT_MOTOR_ENCODER = Encoder.FORWARD;
public static final double FRONT_RIGHT_MOTOR_ENCODER = Encoder.FORWARD;
public static final double BACK_RIGHT_MOTOR_ENCODER = Encoder.FORWARD;
/*
Pedro's parameters
*/
// The weight of the robot in Kilograms
public static final double ROBOT_WEIGHT_IN_KG = 10.5;
public static final double ROBOT_WEIGHT_IN_KG = 5.15;
// Maximum velocity of the robot going forward
public static final double ROBOT_SPEED_FORWARD = 51.4598;
public static final double ROBOT_SPEED_FORWARD = 66.6117;
// Maximum velocity of the robot going right
public static final double ROBOT_SPEED_LATERAL = 28.7119;
public static final double ROBOT_SPEED_LATERAL = 60.0671;
// Rate of deceleration when power is cut-off when the robot is moving forward
public static final double FORWARD_ZERO_POWER_ACCEL = -57.805;
public static final double FORWARD_ZERO_POWER_ACCEL = -71.154;
// Rate of deceleration when power is cut-off when the robot is moving to the right
public static final double LATERAL_ZERO_POWER_ACCEL = -99.672;
public static final double LATERAL_ZERO_POWER_ACCEL = -109.5358;
// Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
public static final double ZERO_POWER_ACCEL_MULT = 3.5;
public static final double ZERO_POWER_ACCEL_MULT = 4.0;
/* Centripetal force correction - increase if robot is correcting into the path
- decrease if robot is correcting away from the path */
public static final double CENTRIPETAL_SCALING = 0.0004;
public static final double CENTRIPETAL_SCALING = 0.0005;
}

171
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorIMUOrthogonal.java
View File

@ -1,171 +0,0 @@
/* Copyright (c) 2022 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_LOGO_FACING_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_USB_FACING_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
/*
* This OpMode shows how to use the new universal IMU interface. This
* interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
* on the robot with the name "imu".
*
* The sample will display the current Yaw, Pitch and Roll of the robot.<br>
* With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
* Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
* Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
* Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
*
* The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
*
* This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
* (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
*
* Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
* the alternative SensorIMUNonOrthogonal sample in this folder.
*
* This "Orthogonal" requirement means that:
*
* 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
*
* 2) The USB ports can only be pointing in one of the same six directions:<br>
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
*
* So, To fully define how your Hub is mounted to the robot, you must simply specify:<br>
* logoFacingDirection<br>
* usbFacingDirection
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
*
* Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
* to use those parameters.
*/
@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
@Disabled // Comment this out to add to the OpMode list
public class SensorIMUOrthogonal extends LinearOpMode {
// The IMU sensor object
IMU imu;
private Encoder leftEncoder;
private Encoder rightEncoder;
private Encoder strafeEncoder;
//----------------------------------------------------------------------------------------------
// Main logic
//----------------------------------------------------------------------------------------------
@Override
public void runOpMode() throws InterruptedException {
// Retrieve and initialize the IMU.
// This sample expects the IMU to be in a REV Hub and named "imu".
imu = hardwareMap.get(IMU.class, PedroConstants.IMU);
// TODO: replace these with your encoder ports
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
/* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
*
* Two input parameters are required to fully specify the Orientation.
* The first parameter specifies the direction the printed logo on the Hub is pointing.
* The second parameter specifies the direction the USB connector on the Hub is pointing.
* All directions are relative to the robot, and left/right is as-viewed from behind the robot.
*
* If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the
* RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection.
*/
/* The next two lines define Hub orientation.
* The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
*
* To Do: EDIT these two lines to match YOUR mounting configuration.
*/
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = IMU_LOGO_FACING_DIRECTION;
RevHubOrientationOnRobot.UsbFacingDirection usbDirection = IMU_USB_FACING_DIRECTION;
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
// Now initialize the IMU with this mounting orientation
// Note: if you choose two conflicting directions, this initialization will cause a code exception.
imu.initialize(new IMU.Parameters(orientationOnRobot));
// Loop and update the dashboard
while (!isStopRequested()) {
telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection);
// Check to see if heading reset is requested
if (gamepad1.y) {
telemetry.addData("Yaw", "Resetting\n");
imu.resetYaw();
} else {
telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
}
// Retrieve Rotational Angles and Velocities
YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
telemetry.update();
}
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
View File

@ -6,9 +6,6 @@ import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.DriveEncoderLocalizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelIMULocalizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.TwoWheelLocalizer;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
@ -70,8 +67,7 @@ public class PoseUpdater {
*/
public PoseUpdater(HardwareMap hardwareMap) {
// TODO: replace the second argument with your preferred localizer
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
// this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
this(hardwareMap, new DriveEncoderLocalizer(hardwareMap));
}
/**

75
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java
View File

@ -1,11 +1,20 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_ENCODER;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -26,21 +35,21 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
*/
@Config
public class DriveEncoderLocalizer extends Localizer {
private HardwareMap hardwareMap;
private final HardwareMap hardwareMap;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private final NanoTimer timer;
private long deltaTimeNano;
private Encoder leftFront;
private Encoder rightFront;
private Encoder leftRear;
private Encoder rightRear;
private final Encoder leftFront;
private final Encoder rightFront;
private final Encoder leftRear;
private final Encoder rightRear;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = -0.6308;
public static double STRAFE_TICKS_TO_INCHES = 46.4839;
public static double TURN_TICKS_TO_RADIANS = -0.002;
public static double FORWARD_TICKS_TO_INCHES = -0.0058;
public static double STRAFE_TICKS_TO_INCHES = -0.0054;
public static double TURN_TICKS_TO_RADIANS = -0.0009;
public static double ROBOT_WIDTH = 1;
public static double ROBOT_LENGTH = 1;
@ -58,22 +67,32 @@ public class DriveEncoderLocalizer extends Localizer {
* This creates a new DriveEncoderLocalizer from a HardwareMap and a Pose, with the Pose
* specifying the starting pose of the localizer.
*
* @param map the HardwareMap
* @param map the HardwareMap
* @param setStartPose the Pose to start from
*/
public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) {
hardwareMap = map;
leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, leftFrontMotorName));
leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, leftRearMotorName));
rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, rightRearMotorName));
rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, rightFrontMotorName));
leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR));
leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR));
rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR));
rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR));
DcMotor leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
DcMotor leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
leftFrontDrive.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
leftBackDrive.setDirection(BACK_LEFT_MOTOR_DIRECTION);
rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
// TODO: reverse any encoders necessary
leftFront.setDirection(Encoder.REVERSE);
rightRear.setDirection(Encoder.REVERSE);
leftRear.setDirection(Encoder.FORWARD);
rightRear.setDirection(Encoder.FORWARD);
leftFront.setDirection(FRONT_LEFT_MOTOR_ENCODER);
rightFront.setDirection(FRONT_RIGHT_MOTOR_ENCODER);
leftRear.setDirection(BACK_LEFT_MOTOR_ENCODER);
rightRear.setDirection(BACK_RIGHT_MOTOR_ENCODER);
setStartPose(setStartPose);
timer = new NanoTimer();
@ -129,7 +148,7 @@ public class DriveEncoderLocalizer extends Localizer {
* @param heading the rotation of the Matrix
*/
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix = new Matrix(3, 3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
@ -164,7 +183,7 @@ public class DriveEncoderLocalizer extends Localizer {
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
Matrix transformation = new Matrix(3, 3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
@ -214,13 +233,13 @@ public class DriveEncoderLocalizer extends Localizer {
* @return returns a Matrix containing the robot relative movement.
*/
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
Matrix returnMatrix = new Matrix(3, 1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition()));
returnMatrix.set(0, 0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition()));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition()));
returnMatrix.set(1, 0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition()));
// theta/turning
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH)));
returnMatrix.set(2, 0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH)));
return returnMatrix;
}

317
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java
View File

@ -1,317 +0,0 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
import static org.firstinspires.ftc.teamcode.PedroConstants.*;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
/**
* This is the ThreeWheelIMULocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the three wheel odometry set up with the IMU to have more accurate heading
* readings. The diagram below, which is modified from Road Runner, shows a typical set up.
*
* The view is from the top of the robot looking downwards.
*
* left on robot is the y positive direction
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
*
* @author Logan Nash
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 7/9/2024
*/
@Config
public class ThreeWheelIMULocalizer extends Localizer {
private HardwareMap hardwareMap;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private long deltaTimeNano;
private Encoder leftEncoder;
private Encoder rightEncoder;
private Encoder strafeEncoder;
private Pose leftEncoderPose;
private Pose rightEncoderPose;
private Pose strafeEncoderPose;
public final IMU imu;
private double previousIMUOrientation;
private double deltaRadians;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 0.004;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = -0.0036;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
public static double TURN_TICKS_TO_RADIANS = 0.0043;//8192 * 1.37795 * 2 * Math.PI * 0.5;
public static boolean useIMU = true;
/**
* This creates a new ThreeWheelIMULocalizer from a HardwareMap, with a starting Pose at (0,0)
* facing 0 heading.
*
* @param map the HardwareMap
*/
public ThreeWheelIMULocalizer(HardwareMap map) {
this(map, new Pose());
}
/**
* This creates a new ThreeWheelIMULocalizer from a HardwareMap and a Pose, with the Pose
* specifying the starting pose of the localizer.
*
* @param map the HardwareMap
* @param setStartPose the Pose to start from
*/
public ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) {
hardwareMap = map;
imu = hardwareMap.get(IMU.class, IMU);
// TODO: replace this with your IMU's orientation
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(IMU_LOGO_FACING_DIRECTION, IMU_USB_FACING_DIRECTION)));
// TODO: replace these with your encoder positions
leftEncoderPose = new Pose(-7.625, 6.19375, 0);
rightEncoderPose = new Pose(-7.625, -6.19375, 0);
strafeEncoderPose = new Pose(7, 1, Math.toRadians(90));
// TODO: replace these with your encoder ports
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
totalHeading = 0;
resetEncoders();
}
/**
* This returns the current pose estimate.
*
* @return returns the current pose estimate as a Pose
*/
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, displacementPose);
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Pose
*/
@Override
public Pose getVelocity() {
return currentVelocity.copy();
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Vector
*/
@Override
public Vector getVelocityVector() {
return currentVelocity.getVector();
}
/**
* This sets the start pose. Changing the start pose should move the robot as if all its
* previous movements were displacing it from its new start pose.
*
* @param setStart the new start pose
*/
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
/**
* This sets the Matrix that contains the previous pose's heading rotation.
*
* @param heading the rotation of the Matrix
*/
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
prevRotationMatrix.set(1, 1, Math.cos(heading));
prevRotationMatrix.set(2, 2, 1.0);
}
/**
* This sets the current pose estimate. Changing this should just change the robot's current
* pose estimate, not anything to do with the start pose.
*
* @param setPose the new current pose estimate
*/
@Override
public void setPose(Pose setPose) {
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
resetEncoders();
}
/**
* This updates the elapsed time timer that keeps track of time between updates, as well as the
* change position of the Encoders. Then, the robot's global change in position is calculated
* using the pose exponential method.
*/
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
updateEncoders();
Matrix robotDeltas = getRobotDeltas();
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(2, 2, 1.0);
} else {
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(2, 2, 1.0);
}
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
totalHeading += globalDeltas.get(2, 0);
}
/**
* This updates the Encoders.
*/
public void updateEncoders() {
leftEncoder.update();
rightEncoder.update();
strafeEncoder.update();
double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation);
previousIMUOrientation = currentIMUOrientation;
}
/**
* This resets the Encoders.
*/
public void resetEncoders() {
leftEncoder.reset();
rightEncoder.reset();
strafeEncoder.reset();
}
/**
* This calculates the change in position from the perspective of the robot using information
* from the Encoders.
*
* @return returns a Matrix containing the robot relative movement.
*/
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))));
// theta/turning
if (MathFunctions.getSmallestAngleDifference(0, deltaRadians) > 0.00005 && useIMU) {
returnMatrix.set(2, 0, deltaRadians);
} else {
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
}
return returnMatrix;
}
/**
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
*
* @return returns how far the robot has turned in total, in radians.
*/
public double getTotalHeading() {
return totalHeading;
}
/**
* This returns the multiplier applied to forward movement measurement to convert from encoder
* ticks to inches. This is found empirically through a tuner.
*
* @return returns the forward ticks to inches multiplier
*/
public double getForwardMultiplier() {
return FORWARD_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
* encoder ticks to inches. This is found empirically through a tuner.
*
* @return returns the lateral/strafe ticks to inches multiplier
*/
public double getLateralMultiplier() {
return STRAFE_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to turning movement measurement to convert from encoder
* ticks to radians. This is found empirically through a tuner.
*
* @return returns the turning ticks to radians multiplier
*/
public double getTurningMultiplier() {
return TURN_TICKS_TO_RADIANS;
}
/**
* This resets the IMU.
*/
public void resetIMU() {
imu.resetYaw();
}
}

297
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java
View File

@ -1,297 +0,0 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
import static org.firstinspires.ftc.teamcode.PedroConstants.*;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
/**
* This is the ThreeWheelLocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the three wheel odometry set up. The diagram below, which is modified from
* Road Runner, shows a typical set up.
*
* The view is from the top of the robot looking downwards.
*
* left on robot is the y positive direction
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
*/
@Config
public class ThreeWheelLocalizer extends Localizer {
private HardwareMap hardwareMap;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private long deltaTimeNano;
private Encoder leftEncoder;
private Encoder rightEncoder;
private Encoder strafeEncoder;
private Pose leftEncoderPose;
private Pose rightEncoderPose;
private Pose strafeEncoderPose;
private double totalHeading;
// public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
// public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
// public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5;
public static double TURN_TICKS_TO_RADIANS = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5;
/**
* This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
* facing 0 heading.
*
* @param map the HardwareMap
*/
public ThreeWheelLocalizer(HardwareMap map) {
this(map, new Pose());
}
/**
* This creates a new ThreeWheelLocalizer from a HardwareMap and a Pose, with the Pose
* specifying the starting pose of the localizer.
*
* @param map the HardwareMap
* @param setStartPose the Pose to start from
*/
public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
// TODO: replace these with your encoder positions
leftEncoderPose = new Pose(0, 6.19375, 0);
rightEncoderPose = new Pose(0, -6.19375, 0);
strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
hardwareMap = map;
// TODO: replace these with your encoder ports
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
totalHeading = 0;
resetEncoders();
}
/**
* This returns the current pose estimate.
*
* @return returns the current pose estimate as a Pose
*/
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, displacementPose);
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Pose
*/
@Override
public Pose getVelocity() {
return currentVelocity.copy();
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Vector
*/
@Override
public Vector getVelocityVector() {
return currentVelocity.getVector();
}
/**
* This sets the start pose. Changing the start pose should move the robot as if all its
* previous movements were displacing it from its new start pose.
*
* @param setStart the new start pose
*/
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
/**
* This sets the Matrix that contains the previous pose's heading rotation.
*
* @param heading the rotation of the Matrix
*/
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
prevRotationMatrix.set(1, 1, Math.cos(heading));
prevRotationMatrix.set(2, 2, 1.0);
}
/**
* This sets the current pose estimate. Changing this should just change the robot's current
* pose estimate, not anything to do with the start pose.
*
* @param setPose the new current pose estimate
*/
@Override
public void setPose(Pose setPose) {
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
resetEncoders();
}
/**
* This updates the elapsed time timer that keeps track of time between updates, as well as the
* change position of the Encoders. Then, the robot's global change in position is calculated
* using the pose exponential method.
*/
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
updateEncoders();
Matrix robotDeltas = getRobotDeltas();
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(2, 2, 1.0);
} else {
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(2, 2, 1.0);
}
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
totalHeading += globalDeltas.get(2, 0);
}
/**
* This updates the Encoders.
*/
public void updateEncoders() {
leftEncoder.update();
rightEncoder.update();
strafeEncoder.update();
}
/**
* This resets the Encoders.
*/
public void resetEncoders() {
leftEncoder.reset();
rightEncoder.reset();
strafeEncoder.reset();
}
/**
* This calculates the change in position from the perspective of the robot using information
* from the Encoders.
*
* @return returns a Matrix containing the robot relative movement.
*/
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))));
// theta/turning
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
return returnMatrix;
}
/**
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
*
* @return returns how far the robot has turned in total, in radians.
*/
public double getTotalHeading() {
return totalHeading;
}
/**
* This returns the multiplier applied to forward movement measurement to convert from encoder
* ticks to inches. This is found empirically through a tuner.
*
* @return returns the forward ticks to inches multiplier
*/
public double getForwardMultiplier() {
return FORWARD_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
* encoder ticks to inches. This is found empirically through a tuner.
*
* @return returns the lateral/strafe ticks to inches multiplier
*/
public double getLateralMultiplier() {
return STRAFE_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to turning movement measurement to convert from encoder
* ticks to radians. This is found empirically through a tuner.
*
* @return returns the turning ticks to radians multiplier
*/
public double getTurningMultiplier() {
return TURN_TICKS_TO_RADIANS;
}
/**
* This does nothing since this localizer does not use the IMU.
*/
public void resetIMU() {
}
}

302
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java
View File

@ -1,302 +0,0 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
/**
* This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from
* Road Runner, shows a typical set up.
*
* The view is from the top of the robot looking downwards.
*
* left on robot is the y positive direction
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
*/
@Config
public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo work
private HardwareMap hardwareMap;
private IMU imu;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private long deltaTimeNano;
private Encoder forwardEncoder;
private Encoder strafeEncoder;
private Pose forwardEncoderPose;
private Pose strafeEncoderPose;
private double previousIMUOrientation;
private double deltaRadians;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
/**
* This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
* facing 0 heading.
*
* @param map the HardwareMap
*/
public TwoWheelLocalizer(HardwareMap map) {
this(map, new Pose());
}
/**
* This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose
* specifying the starting pose of the localizer.
*
* @param map the HardwareMap
* @param setStartPose the Pose to start from
*/
public TwoWheelLocalizer(HardwareMap map, Pose setStartPose) {
// TODO: replace these with your encoder positions
forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0);
strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
hardwareMap = map;
imu = hardwareMap.get(IMU.class, "imu");
// TODO: replace this with your IMU's orientation
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT)));
// TODO: replace these with your encoder ports
forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
forwardEncoder.setDirection(Encoder.REVERSE);
strafeEncoder.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
previousIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = 0;
}
/**
* This returns the current pose estimate.
*
* @return returns the current pose estimate as a Pose
*/
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, displacementPose);
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Pose
*/
@Override
public Pose getVelocity() {
return currentVelocity.copy();
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Vector
*/
@Override
public Vector getVelocityVector() {
return currentVelocity.getVector();
}
/**
* This sets the start pose. Changing the start pose should move the robot as if all its
* previous movements were displacing it from its new start pose.
*
* @param setStart the new start pose
*/
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
/**
* This sets the Matrix that contains the previous pose's heading rotation.
*
* @param heading the rotation of the Matrix
*/
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
prevRotationMatrix.set(1, 1, Math.cos(heading));
prevRotationMatrix.set(2, 2, 1.0);
}
/**
* This sets the current pose estimate. Changing this should just change the robot's current
* pose estimate, not anything to do with the start pose.
*
* @param setPose the new current pose estimate
*/
@Override
public void setPose(Pose setPose) {
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
resetEncoders();
}
/**
* This updates the elapsed time timer that keeps track of time between updates, as well as the
* change position of the Encoders and the IMU readings. Then, the robot's global change in
* position is calculated using the pose exponential method.
*/
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
updateEncoders();
Matrix robotDeltas = getRobotDeltas();
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(2, 2, 1.0);
} else {
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(2, 2, 1.0);
}
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
totalHeading += globalDeltas.get(2, 0);
}
/**
* This updates the Encoders as well as the IMU.
*/
public void updateEncoders() {
forwardEncoder.update();
strafeEncoder.update();
double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation);
previousIMUOrientation = currentIMUOrientation;
}
/**
* This resets the Encoders.
*/
public void resetEncoders() {
forwardEncoder.reset();
strafeEncoder.reset();
}
/**
* This calculates the change in position from the perspective of the robot using information
* from the Encoders and IMU.
*
* @return returns a Matrix containing the robot relative movement.
*/
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians));
// theta/turning
returnMatrix.set(2,0, deltaRadians);
return returnMatrix;
}
/**
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
*
* @return returns how far the robot has turned in total, in radians.
*/
public double getTotalHeading() {
return totalHeading;
}
/**
* This returns the multiplier applied to forward movement measurement to convert from encoder
* ticks to inches. This is found empirically through a tuner.
*
* @return returns the forward ticks to inches multiplier
*/
public double getForwardMultiplier() {
return FORWARD_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
* encoder ticks to inches. This is found empirically through a tuner.
*
* @return returns the lateral/strafe ticks to inches multiplier
*/
public double getLateralMultiplier() {
return STRAFE_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to turning movement measurement to convert from encoder
* ticks to radians. This is found empirically through a tuner.
*
* @return returns the turning ticks to radians multiplier
*/
public double getTurningMultiplier() {
return 1;
}
/**
* This resets the IMU.
*/
public void resetIMU() {
imu.resetYaw();
}
}

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java
View File

@ -58,9 +58,6 @@ public class LocalizationTest extends OpMode {
rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
for (DcMotorEx motor : motors) {

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java
View File

@ -42,7 +42,7 @@ public class FollowerConstants {
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
0.1,
0,
0.01,
0,
0);
// Translational Integral
@ -60,7 +60,7 @@ public class FollowerConstants {
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
1,
0,
.1,
0,
0);
// Feed forward constant added on to the heading PIDF
@ -69,10 +69,10 @@ public class FollowerConstants {
// Drive PIDF coefficients
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
0.006,
0.025,
0,
0.00001,
0.8,
0.6,
0);
// Feed forward constant added on to the drive PIDF
@ -202,9 +202,9 @@ public class FollowerConstants {
// Secondary drive PIDF coefficients
public static CustomFilteredPIDFCoefficients secondaryDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
0.02,
0.00315,
0,
0.000005,
0.0001,
0.6,
0);