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

Updated movement library to the updated orientation of the motors, added speed control via bumpers

Browse Source
This commit is contained in:
Carlos
2024-06-22 11:02:18 -07:00
parent 9e8609e68d
commit 4ecd419511
2 changed files with 46 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,6 +33,7 @@ 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;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
@ -79,6 +80,9 @@ public class CometBotsLinearOpMode extends LinearOpMode {
@Override
public void runOpMode() {
// 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, "right hand wheel");
@ -116,22 +120,22 @@ public class CometBotsLinearOpMode extends LinearOpMode {
// 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 rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
double lftFrntPwr = axial + lateral + yaw;
double rtFrntPwr = axial - lateral - yaw;
double ltBckPwr = axial - lateral + yaw;
double rtBckPwr = axial + lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
max = Math.max(Math.abs(lftFrntPwr), Math.abs(rtFrntPwr));
max = Math.max(max, Math.abs(ltBckPwr));
max = Math.max(max, Math.abs(rtBckPwr));
if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
lftFrntPwr /= max;
rtFrntPwr /= max;
ltBckPwr /= max;
rtBckPwr /= max;
}
// This is test code:
@ -145,22 +149,39 @@ public class CometBotsLinearOpMode extends LinearOpMode {
// Once the correct motors move in the correct direction re-comment this code.
/*
leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad
leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad
rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad
rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad
lftFrntPwr = gamepad1.x ? 1.0 : 0.0; // X gamepad
ltBckPwr = gamepad1.a ? 1.0 : 0.0; // A gamepad
rtFrntPwr = gamepad1.y ? 1.0 : 0.0; // Y gamepad
rtBckPwr = gamepad1.b ? 1.0 : 0.0; // B gamepad
*/
lhw.setPower(lftFrntPwr/cntBtnA);
rhw.setPower(rtFrntPwr/cntBtnA);
llw.setPower(ltBckPwr/cntBtnA);
rlw.setPower(rtBckPwr/cntBtnA);
// Send calculated power to wheels
lhw.setPower(leftFrontPower);
rhw.setPower(rightFrontPower);
llw.setPower(leftBackPower);
rlw.setPower(rightBackPower);
if(gamepad1.left_bumper) {
sleep(175);
cntBtnA--;
if(cntBtnA < 0) {
cntBtnA = 0;
}
}
if(gamepad1.right_bumper) {
sleep(175);
cntBtnA++;
if(cntBtnA > 4) {
cntBtnA = 4;
}
}
// Show the elapsed game time and wheel power.
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);
telemetry.addData("Front left/Right", "%4.2f, %4.2f", lftFrntPwr, rtFrntPwr);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", ltBckPwr, rtBckPwr);
telemetry.addData("Power Mode", cntBtnA);
telemetry.update();
}
}}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java
View File

@ -12,21 +12,21 @@ public class MovementLibrary {
public static void setLeftSideForward(DcMotor[] motors) {
motors[2].setDirection(DcMotor.Direction.FORWARD);
motors[3].setDirection(DcMotor.Direction.REVERSE);
motors[3].setDirection(DcMotor.Direction.FORWARD);
}
public static void setRightSideForward(DcMotor[] motors) {
motors[0].setDirection(DcMotor.Direction.REVERSE);
motors[0].setDirection(DcMotor.Direction.FORWARD);
motors[1].setDirection(DcMotor.Direction.FORWARD);
}
public static void setLeftSideBackward(DcMotor[] motors) {
motors[2].setDirection(DcMotor.Direction.REVERSE);
motors[3].setDirection(DcMotor.Direction.FORWARD);
motors[3].setDirection(DcMotor.Direction.REVERSE);
}
public static void setRightSideBackward(DcMotor[] motors) {
motors[0].setDirection(DcMotor.Direction.FORWARD);
motors[0].setDirection(DcMotor.Direction.REVERSE);
motors[1].setDirection(DcMotor.Direction.REVERSE);
}