Merge branch 'branch-rc-chassis-14493' into branch-black-14493
This commit is contained in:
@ -0,0 +1,122 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExample", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExample extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(12.0, 11, 90);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.6);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(12.804, 11.223, Point.CARTESIAN),
|
||||
new Point(12.804, 42.362, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierCurve(
|
||||
new Point(12.804, 42.362, Point.CARTESIAN),
|
||||
new Point(11.381, 57.379, Point.CARTESIAN),
|
||||
new Point(31.614, 56.588, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(31.614, 56.588, Point.CARTESIAN),
|
||||
new Point(51.214, 56.746, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierCurve(
|
||||
new Point(51.214, 56.746, Point.CARTESIAN),
|
||||
new Point(64.334, 58.643, Point.CARTESIAN),
|
||||
new Point(61.172, 45.524, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierCurve(
|
||||
new Point(61.172, 45.524, Point.CARTESIAN),
|
||||
new Point(36.198, 26.239, Point.CARTESIAN),
|
||||
new Point(19.759, 11.065, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,95 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExampleTwo", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExampleTwo extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(10.0, 40, 90);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.4);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(10.000, 40.000, Point.CARTESIAN),
|
||||
new Point(60.000, 40.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(60.000, 40.000, Point.CARTESIAN),
|
||||
new Point(60.000, 25.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(60.000, 25.000, Point.CARTESIAN),
|
||||
new Point(10.000, 25.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -29,6 +29,11 @@
|
||||
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
||||
>>>>>>> branch-rc-chassis-14493
|
||||
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;
|
||||
@ -37,13 +42,28 @@ 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;
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
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;
|
||||
>>>>>>> branch-rc-chassis-14493
|
||||
|
||||
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;
|
||||
<<<<<<< HEAD
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
=======
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
||||
|
||||
>>>>>>> branch-rc-chassis-14493
|
||||
/*
|
||||
* This file contains an example of a Linear "OpMode".
|
||||
* An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
|
||||
@ -76,21 +96,44 @@ import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
public class BasicOmniOpMode_Linear extends LinearOpMode {
|
||||
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
<<<<<<< HEAD
|
||||
private ElapsedTime runtime = new ElapsedTime();
|
||||
private DcMotor leftFrontDrive = null;
|
||||
private DcMotor leftBackDrive = null;
|
||||
private DcMotor rightFrontDrive = null;
|
||||
private DcMotor rightBackDrive = null;
|
||||
=======
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
>>>>>>> branch-rc-chassis-14493
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
// Initialize the hardware variables. Note that the strings used here must correspond
|
||||
// to the names assigned during the robot configuration step on the DS or RC devices.
|
||||
<<<<<<< HEAD
|
||||
leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
|
||||
leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
|
||||
rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
|
||||
rightBackDrive = hardwareMap.get(DcMotor.class, BACK_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);
|
||||
|
||||
|
||||
|
||||
// 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);
|
||||
>>>>>>> branch-rc-chassis-14493
|
||||
|
||||
// ########################################################################################
|
||||
// !!! IMPORTANT Drive Information. Test your motor directions. !!!!!
|
||||
@ -109,6 +152,12 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
||||
|
||||
// Wait for the game to start (driver presses START)
|
||||
telemetry.addData("Status", "Initialized");
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
|
||||
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
|
||||
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
|
||||
>>>>>>> branch-rc-chassis-14493
|
||||
telemetry.update();
|
||||
|
||||
waitForStart();
|
||||
@ -170,6 +219,12 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
|
||||
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
|
||||
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
|
||||
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
|
||||
>>>>>>> branch-rc-chassis-14493
|
||||
telemetry.update();
|
||||
}
|
||||
}}
|
||||
|
||||
@ -10,6 +10,8 @@ public class PedroConstants {
|
||||
/*
|
||||
Robot parameters
|
||||
*/
|
||||
// Turn localizer - -0.003
|
||||
|
||||
|
||||
// Robot motor configurations
|
||||
public static final String FRONT_LEFT_MOTOR = "Drive front lt";
|
||||
@ -40,7 +42,7 @@ public class PedroConstants {
|
||||
// Robot encoder direction
|
||||
public static final double LEFT_ENCODER_DIRECTION = Encoder.REVERSE;
|
||||
public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
|
||||
public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE;
|
||||
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
|
||||
|
||||
/*
|
||||
Pedro's parameters
|
||||
@ -51,21 +53,21 @@ public class PedroConstants {
|
||||
public static final double ROBOT_WEIGHT_IN_KG = 10.5;
|
||||
|
||||
// Maximum velocity of the robot going forward
|
||||
public static final double ROBOT_SPEED_FORWARD = 61.7259;
|
||||
public static final double ROBOT_SPEED_FORWARD = 51.4598;
|
||||
|
||||
// Maximum velocity of the robot going right
|
||||
public static final double ROBOT_SPEED_LATERAL = 2;
|
||||
public static final double ROBOT_SPEED_LATERAL = 28.7119;
|
||||
|
||||
// Rate of deceleration when power is cut-off when the robot is moving forward
|
||||
public static final double FORWARD_ZERO_POWER_ACCEL = -74.3779;
|
||||
public static final double FORWARD_ZERO_POWER_ACCEL = -57.805;
|
||||
|
||||
// Rate of deceleration when power is cut-off when the robot is moving to the right
|
||||
public static final double LATERAL_ZERO_POWER_ACCEL = -111.8409;
|
||||
public static final double LATERAL_ZERO_POWER_ACCEL = -99.672;
|
||||
|
||||
// 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 = 4;
|
||||
public static final double ZERO_POWER_ACCEL_MULT = 3.5;
|
||||
|
||||
/* 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.0005;
|
||||
public static final double CENTRIPETAL_SCALING = 0.0004;
|
||||
}
|
||||
|
||||
@ -0,0 +1,171 @@
|
||||
/* 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -71,7 +71,8 @@ measurements will be in centimeters.
|
||||
of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but
|
||||
what works best for you is most important. Higher numbers will cause a faster brake, but increase
|
||||
oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more sensitive than the others. For reference,
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more
|
||||
* sensitive than the others. For reference,
|
||||
my P values were in the hundredths and thousandths place values, and my D values were in the hundred
|
||||
thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and
|
||||
`useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`
|
||||
|
||||
@ -5,6 +5,7 @@ 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.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;
|
||||
@ -69,7 +70,8 @@ public class PoseUpdater {
|
||||
*/
|
||||
public PoseUpdater(HardwareMap hardwareMap) {
|
||||
// TODO: replace the second argument with your preferred localizer
|
||||
this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
|
||||
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
|
||||
// this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -38,9 +38,9 @@ public class DriveEncoderLocalizer extends Localizer {
|
||||
private Encoder leftRear;
|
||||
private Encoder rightRear;
|
||||
private double totalHeading;
|
||||
public static double FORWARD_TICKS_TO_INCHES = 1;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 1;
|
||||
public static double TURN_TICKS_TO_RADIANS = 1;
|
||||
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 ROBOT_WIDTH = 1;
|
||||
public static double ROBOT_LENGTH = 1;
|
||||
|
||||
|
||||
@ -57,9 +57,12 @@ public class ThreeWheelLocalizer extends Localizer {
|
||||
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 STRAFE_TICKS_TO_INCHES = 0.00052189;//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 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)
|
||||
@ -80,9 +83,9 @@ public class ThreeWheelLocalizer extends Localizer {
|
||||
*/
|
||||
public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
|
||||
// TODO: replace these with your encoder positions
|
||||
leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0);
|
||||
rightEncoderPose = new Pose(-18.4/25.4 - 0.1, -159.6/25.4, 0);
|
||||
strafeEncoderPose = new Pose(0*(-107.9/25.4+8)+-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
|
||||
leftEncoderPose = new Pose(0, 6.19375, 0);
|
||||
rightEncoderPose = new Pose(0, -6.19375, 0);
|
||||
strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
|
||||
|
||||
hardwareMap = map;
|
||||
|
||||
@ -92,9 +95,9 @@ public class ThreeWheelLocalizer extends Localizer {
|
||||
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
|
||||
|
||||
// TODO: reverse any encoders necessary
|
||||
//leftEncoder.setDirection(Encoder.REVERSE);
|
||||
// rightEncoder.setDirection(Encoder.REVERSE);
|
||||
//strafeEncoder.setDirection(Encoder.FORWARD);
|
||||
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
|
||||
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
|
||||
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
|
||||
|
||||
setStartPose(setStartPose);
|
||||
timer = new NanoTimer();
|
||||
|
||||
@ -63,6 +63,7 @@ public class LateralTuner extends OpMode {
|
||||
telemetryA.addData("distance moved", poseUpdater.getPose().getY());
|
||||
telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
|
||||
telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier()));
|
||||
|
||||
telemetryA.update();
|
||||
|
||||
Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");
|
||||
|
||||
@ -42,7 +42,7 @@ public class FollowerConstants {
|
||||
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
|
||||
0.1,
|
||||
0,
|
||||
0,
|
||||
0.01,
|
||||
0);
|
||||
|
||||
// Translational Integral
|
||||
@ -58,10 +58,10 @@ public class FollowerConstants {
|
||||
|
||||
// Heading error PIDF coefficients
|
||||
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
|
||||
1,
|
||||
2,
|
||||
0,
|
||||
0,
|
||||
0);
|
||||
.075,
|
||||
-.03125);
|
||||
|
||||
// Feed forward constant added on to the heading PIDF
|
||||
public static double headingPIDFFeedForward = 0.01;
|
||||
@ -69,10 +69,10 @@ public class FollowerConstants {
|
||||
|
||||
// Drive PIDF coefficients
|
||||
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
|
||||
0.025,
|
||||
0.006,
|
||||
0,
|
||||
0.00001,
|
||||
0.6,
|
||||
0.8,
|
||||
0);
|
||||
|
||||
// Feed forward constant added on to the drive PIDF
|
||||
|
||||
@ -119,6 +119,13 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
|
||||
telemetry.addData("x",poseUpdater.getPose().getX());
|
||||
telemetry.addData("y",poseUpdater.getPose().getY());
|
||||
telemetry.addData("heading",poseUpdater.getPose().getHeading());
|
||||
telemetry.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
|
||||
telemetry.addData("velo theta", poseUpdater.getVelocity().getTheta());
|
||||
|
||||
if (gamepad1.cross || gamepad1.a) {
|
||||
requestOpModeStop();
|
||||
}
|
||||
|
||||
@ -6,14 +6,14 @@ repositories {
|
||||
}
|
||||
|
||||
dependencies {
|
||||
implementation 'org.firstinspires.ftc:Inspection:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:Blocks:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:RobotCore:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:RobotServer:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:OnBotJava:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:Hardware:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:FtcCommon:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:Vision:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:Inspection:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:Blocks:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:RobotCore:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:RobotServer:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:OnBotJava:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:Hardware:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:FtcCommon:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:Vision:10.1.0'
|
||||
implementation 'androidx.appcompat:appcompat:1.2.0'
|
||||
|
||||
implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
|
||||
|
||||
Reference in New Issue
Block a user