|
|
|
|
@ -1,172 +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.cometbots;
|
|
|
|
|
|
|
|
|
|
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.PedroConstants;
|
|
|
|
|
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();
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
