SCDS/FtcRobotControllerSDK
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Updating to use Roadrunner

Browse Source
This commit is contained in:
Carlos
2024-06-30 09:57:10 -07:00
parent 1a1ef3b2dc
commit b779a61a7c
4 changed files with 173 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java
View File

@ -29,6 +29,8 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.cometbots.Constants.*;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@ -85,10 +87,10 @@ public class CometBotsLinearOpMode extends LinearOpMode {
// 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.
rhw = hardwareMap.get(DcMotor.class, "right hand wheel");
rlw = hardwareMap.get(DcMotor.class, "right leg wheel");
lhw = hardwareMap.get(DcMotor.class, "left hand wheel");
llw = hardwareMap.get(DcMotor.class, "left leg wheel");
rhw = hardwareMap.get(DcMotor.class, RT_FRONT);
rlw = hardwareMap.get(DcMotor.class, RT_BACK);
lhw = hardwareMap.get(DcMotor.class, LT_FRONT);
llw = hardwareMap.get(DcMotor.class, LT_BACK);
/*
Put all motors in an array for easier handling

132
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsMecanumDriveMode.java Normal file
View File

@ -0,0 +1,132 @@
/* Copyright (c) 2021 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.cometbots.Constants.LT_BACK;
import static org.firstinspires.ftc.teamcode.cometbots.Constants.LT_FRONT;
import static org.firstinspires.ftc.teamcode.cometbots.Constants.RT_BACK;
import static org.firstinspires.ftc.teamcode.cometbots.Constants.RT_FRONT;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary;
/*
* 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.
* The names of OpModes appear on the menu of the FTC Driver Station.
* When a selection is made from the menu, the corresponding OpMode is executed.
*
* This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot.
* This code will work with either a Mecanum-Drive or an X-Drive train.
* Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html
* Note that a Mecanum drive must display an X roller-pattern when viewed from above.
*
* Also note that it is critical to set the correct rotation direction for each motor. See details below.
*
* Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously.
* Each motion axis is controlled by one Joystick axis.
*
* 1) Axial: Driving forward and backward Left-joystick Forward/Backward
* 2) Lateral: Strafing right and left Left-joystick Right and Left
* 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left
*
* This code is written assuming that the right-side motors need to be reversed for the robot to drive forward.
* When you first test your robot, if it moves backward when you push the left stick forward, then you must flip
* the direction of all 4 motors (see code below).
*
* 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
*/
@TeleOp(name="CometBots Mecanum Drive Demo", group = "[14493] CometBots")
public class CometBotsMecanumDriveMode extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private ElapsedTime runtime = new ElapsedTime();
private DcMotor rhw = null;
private DcMotor rlw = null;
private DcMotor lhw = null;
private DcMotor llw = null;
@Override
public void runOpMode() {
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,Math.toRadians(90)));
// Button A counter to cycle speeds
int cntBtnA = 1;
// 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.
rhw = hardwareMap.get(DcMotor.class, RT_FRONT);
rlw = hardwareMap.get(DcMotor.class, RT_BACK);
lhw = hardwareMap.get(DcMotor.class, LT_FRONT);
llw = hardwareMap.get(DcMotor.class, LT_BACK);
/*
Put all motors in an array for easier handling
NOTE: Placement of motor in array is important especially when dealing with functions
that set direction
*/
DcMotor[] motors = {rhw, rlw, lhw, llw};
/*
Initialize all wheels forward using motors a
*/
MovementLibrary.setDirectionForward(motors);
telemetry.addData("Status", "Initialized");
telemetry.update();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
drive.setDrivePowers(
new PoseVelocity2d(
new Vector2d(
gamepad1.right_stick_x,
gamepad1.right_stick_y
),
-gamepad1.right_stick_x
)
);
drive.updatePoseEstimate();
}
}}

47
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View File

@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.cometbots.Constants.*;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.canvas.Canvas;
@ -41,6 +43,7 @@ import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary;
import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage;
@ -58,19 +61,19 @@ public final class MecanumDrive {
// TODO: fill in these values based on
// see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
RevHubOrientationOnRobot.LogoFacingDirection.UP;
RevHubOrientationOnRobot.LogoFacingDirection.FORWARD;
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
RevHubOrientationOnRobot.UsbFacingDirection.UP;
// drive model parameters
public double inPerTick = 1;
public double lateralInPerTick = inPerTick;
public double trackWidthTicks = 0;
public double inPerTick = 119/5924.5;
public double lateralInPerTick = 124/6930.5;
public double trackWidthTicks = 1131;
// feedforward parameters (in tick units)
public double kS = 0;
public double kV = 0;
public double kA = 0;
public double kS = 0.7476;
public double kV = 0.0043736;
public double kA = 00055;
// path profile parameters (in inches)
public double maxWheelVel = 50;
@ -82,9 +85,9 @@ public final class MecanumDrive {
public double maxAngAccel = Math.PI;
// path controller gains
public double axialGain = 0.0;
public double lateralGain = 0.0;
public double headingGain = 0.0; // shared with turn
public double axialGain = 4.125;
public double lateralGain = 4.125;
public double headingGain = 4.25; // shared with turn
public double axialVelGain = 0.0;
public double lateralVelGain = 0.0;
@ -217,18 +220,28 @@ public final class MecanumDrive {
// TODO: make sure your config has motors with these names (or change them)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
leftFront = hardwareMap.get(DcMotorEx.class, LT_FRONT);
leftBack = hardwareMap.get(DcMotorEx.class, LT_BACK);
rightBack = hardwareMap.get(DcMotorEx.class, RT_BACK);
rightFront = hardwareMap.get(DcMotorEx.class, RT_FRONT);
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// TODO: reverse motor directions if needed
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
/*
Put all motors in an array for easier handling
NOTE: Placement of motor in array is important especially when dealing with functions
that set direction
*/
DcMotor[] motors = {rightFront, rightBack, leftFront, leftBack};
/*
Initialize all wheels forward using motors a
*/
MovementLibrary.setDirectionForward(motors);
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/Constants.java
View File

@ -4,4 +4,9 @@ public class Constants {
public static double CB_BASE_SPEED = 0.2;
public static String RT_FRONT = "right hand wheel";
public static String LT_FRONT = "left hand wheel";
public static String RT_BACK = "right leg wheel";
public static String LT_BACK = "left leg wheel";
}