diff --git a/NOTES.md b/NOTES.md new file mode 100644 index 0000000..d2a07b2 --- /dev/null +++ b/NOTES.md @@ -0,0 +1,4 @@ +- Refactor of code + - Possibly establish patterns +- Also, establish github for students as an element of a professional portfolio (laura) +- \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SCDSChassisDriverMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SCDSChassisDriverMode.java new file mode 100644 index 0000000..173612c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SCDSChassisDriverMode.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +@TeleOp( name = "scds-chassis-manual") +public class SCDSChassisDriverMode extends OpMode { + + DcMotor frontRight; + DcMotor backRight; + DcMotor frontLeft; + DcMotor backLeft; + public double axial; + public double lateral; + public double yaw; + + final static double MOTOR_LO_SPEED_RATIO = 3.5; + final static double MOTOR_HI_SPEED_RATIO = 2.25; + final static double ARM_POWER = 3.5; + double RUNNING_MOTOR_SPEED_RATIO = MOTOR_LO_SPEED_RATIO; + double CURRENT_SPEED_RATIO = MOTOR_HI_SPEED_RATIO; + + @Override + public void init() { + telemetry.addData("Status","In Init()"); + telemetry.update(); + frontRight = hardwareMap.dcMotor.get("Drive front rt"); + backRight = hardwareMap.dcMotor.get("Drive back rt"); + frontLeft = hardwareMap.dcMotor.get("Drive front lt"); + backLeft = hardwareMap.dcMotor.get("Drive back lt"); + } + + private void setForwardDirection() { + telemetry.addData("Status","setForwardDirection()"); + telemetry.update(); + frontLeft.setDirection(DcMotor.Direction.REVERSE); + backLeft.setDirection(DcMotor.Direction.FORWARD); + frontRight.setDirection(DcMotor.Direction.REVERSE); + backRight.setDirection(DcMotor.Direction.FORWARD); + } + + @Override + public void loop() { + /* + Initialize the wheels + */ + setForwardDirection(); + + /* + Turn on high speed on the motors + */ + if(gamepad1.a) { + RUNNING_MOTOR_SPEED_RATIO = MOTOR_HI_SPEED_RATIO; + } + + /* + Turn on low speed on the motors + */ + if(gamepad1.b) { + RUNNING_MOTOR_SPEED_RATIO = MOTOR_LO_SPEED_RATIO; + } + + axial = -gamepad1.left_stick_y/CURRENT_SPEED_RATIO; // Note: pushing stick forward gives negative value + lateral = gamepad1.left_stick_x/CURRENT_SPEED_RATIO; + yaw = gamepad1.right_stick_x/CURRENT_SPEED_RATIO; + + // 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; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + frontLeft.setPower(leftFrontPower); + frontRight.setPower(rightFrontPower); + backLeft.setPower(leftBackPower); + backRight.setPower(rightBackPower); + + // Show the elapsed game time and wheel power + telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + + } + +}