Skip to content

Commit

Permalink
Merge pull request #6 from rh-robotics/JackInitialTeleop
Browse files Browse the repository at this point in the history 
Reworked Driving Code
  • Loading branch information
@aurora-cichos
aurora-cichos authored Nov 5, 2024
2 parents a722f55 + 50779fc commit 1b3f057
Show file tree
Hide file tree
Showing 3 changed files with 126 additions and 34 deletions.
54 changes: 42 additions & 12 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,13 @@

import androidx.annotation.NonNull;

import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcore.external.Telemetry;
Expand All @@ -15,9 +18,18 @@
*/
public class HWC {
// Declare empty variables for robot hardware
public DcMotorEx leftFront, rightFront, leftRear, rightRear;
public DcMotorEx leftFront, rightFront, leftRear, rightRear, rightSlide, leftSlide;
//TODO: CHANGE TO NORMAL SERVO
public Servo claw;


// Other Variables
// ------ Declare Gamepads ------ //
public Gamepad currentGamepad1 = new Gamepad();
public Gamepad currentGamepad2 = new Gamepad();
public Gamepad previousGamepad1 = new Gamepad();
public Gamepad previousGamepad2 = new Gamepad();

Telemetry telemetry;
ElapsedTime time = new ElapsedTime();

Expand All @@ -30,12 +42,22 @@ public class HWC {
public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
this.telemetry = telemetry;

// Declare motors
// Declare Driving motors
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");

//Declare Other Motors
leftSlide = hardwareMap.get(DcMotorEx.class, "LSlide");
rightSlide = hardwareMap.get(DcMotorEx.class, "RSlide");

//Declare Servos
claw = hardwareMap.get(Servo.class, "claw");




// Set the direction of motors
// TODO: UPDATE VALUES WITH NEW BOT
leftFront.setDirection(DcMotorEx.Direction.REVERSE);
Expand All @@ -45,16 +67,24 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {

// Set motors to break when power = 0
// TODO: REMOVE IF THIS BEHAVIOUR IS NOT DESIRED ON NEW BOT
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

// Run motors using encoder, so that we can move accurately.
leftFront.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
rightFront.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
leftRear.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
rightRear.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
leftFront.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
leftRear.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
rightRear.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
leftSlide.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
rightSlide.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);

// Set driving motors to run without encoders, we're using odometry instead
leftFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
rightFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
leftRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
rightRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);

// Set slide motors to use encoders
rightSlide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftSlide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightSlide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
leftSlide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
// TODO: ADD ANY HARDWARE RELATED FUNCTIONS BELOW
}
100 changes: 78 additions & 22 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/InitialTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,20 @@
* TeleOp OpMode for simply driving with strafing wheels
* Look at JAVA DOC!
*/
@TeleOp(name = "Basic Strafe Drive", group = "Iterative OpMode")
@TeleOp(name = "INIT TELEOP", group = "Iterative OpMode")
public class InitialTeleOp extends OpMode {
private final ElapsedTime time = new ElapsedTime();
HWC robot; // Declare the object for HWC, will allow us to access all the motors declared there!
TeleOpStates state; // Creates object of states enum
public enum MultiplierSelection {
TURN_SPEED,
DRIVE_SPEED,
STRAFE_SPEED
}
private MultiplierSelection selection = MultiplierSelection.TURN_SPEED;
private double turnSpeed = 0.5; // Speed multiplier for turning
private double driveSpeed = 1; // Speed multiplier for driving
private double strafeSpeed = 0.8; // Speed multiplier for strafing
// init() Runs ONCE after the driver hits initialize
@Override
public void init() {
Expand All @@ -30,14 +39,62 @@ public void init() {
telemetry.addData("Status", "Initialized");

// Creates States
state = TeleOpStates.START;
state = TeleOpStates.START;
}

// init_loop() - Runs continuously until the driver hits play
@Override
public void init_loop() {
robot.previousGamepad1.copy(robot.currentGamepad1);
robot.currentGamepad1.copy(gamepad1);

// ------ Speed Multiplier Selection ------ //
if (robot.currentGamepad1.a && !robot.previousGamepad1.a) {
selection = MultiplierSelection.TURN_SPEED;
} else if (robot.currentGamepad1.b && !robot.previousGamepad1.b) {
selection = MultiplierSelection.DRIVE_SPEED;
} else if (robot.currentGamepad1.x && !robot.previousGamepad1.x) {
selection = MultiplierSelection.STRAFE_SPEED;
}

// ------ Speed Multiplier Changes ------ //
switch (selection) {
case TURN_SPEED:
if (robot.currentGamepad1.dpad_up && !robot.previousGamepad1.dpad_up) {
turnSpeed += 0.1;
} else if (robot.currentGamepad1.dpad_down && !robot.previousGamepad1.dpad_down) {
turnSpeed -= 0.1;
}
break;
case DRIVE_SPEED:
if (robot.currentGamepad1.dpad_up && !robot.previousGamepad1.dpad_up) {
driveSpeed += 0.1;
} else if (robot.currentGamepad1.dpad_down && !robot.previousGamepad1.dpad_down) {
driveSpeed -= 0.1;
}
break;
case STRAFE_SPEED:
if (robot.currentGamepad1.dpad_up && !robot.previousGamepad1.dpad_up) {
strafeSpeed += 0.1;
} else if (robot.currentGamepad1.dpad_down && !robot.previousGamepad1.dpad_down) {
strafeSpeed -= 0.1;
}
break;
}
telemetry.addData("Press A to start changing turn speed", "");
telemetry.addData("Press B to start changing drive speed", "");
telemetry.addData("Press X to start changing strafe speed", "");
telemetry.addLine();
telemetry.addData("Modifying", selection);
telemetry.addLine();
telemetry.addData("Turn Speed", turnSpeed);
telemetry.addData("Drive Speed", driveSpeed);
telemetry.addData("Strafe Speed", strafeSpeed);
telemetry.update();
}



// Start() - Runs ONCE when the driver presses play
@Override
public void start() {
Expand All @@ -51,28 +108,19 @@ public void loop() {
double rightFPower;
double leftBPower;
double rightBPower;
double drive = -gamepad1.left_stick_x * 0.8;
double turn = gamepad1.left_stick_y * 0.6;
double strafe = -gamepad1.right_stick_x * 0.8;
robot.previousGamepad1.copy(robot.currentGamepad1);
robot.currentGamepad1.copy(gamepad1);

// Calculate drive power
if (drive != 0 || turn != 0) {
leftFPower = Range.clip(drive + turn, -1.0, 1.0);
rightFPower = Range.clip(drive - turn, -1.0, 1.0);
leftBPower = Range.clip(drive + turn, -1.0, 1.0);
rightBPower = Range.clip(drive - turn, -1.0, 1.0);
} else if (strafe != 0) {
// Strafing
leftFPower = -strafe;
rightFPower = strafe;
leftBPower = strafe;
rightBPower = -strafe;
} else {
leftFPower = 0;
rightFPower = 0;
leftBPower = 0;
rightBPower = 0;
}
double drive = -robot.currentGamepad1.left_stick_y * driveSpeed;
double strafe = robot.currentGamepad1.left_stick_x * strafeSpeed;
double turn = (robot.currentGamepad1.left_trigger - robot.currentGamepad1.right_trigger) * turnSpeed;

double denominator = Math.max(Math.abs(drive) + Math.abs(strafe) + Math.abs(turn), 1);
leftFPower = (turn - strafe - drive) / denominator;
leftBPower = (turn + strafe - drive) / denominator;
rightFPower = (turn - strafe + drive) / denominator;
rightBPower = (turn + strafe + drive) / denominator;

// Set power to values calculated above
robot.leftFront.setPower(leftFPower);
Expand Down Expand Up @@ -118,6 +166,14 @@ public void loop() {
}

telemetry.addData("State", state);
telemetry.addData("Right Front", rightFPower);
telemetry.addData("Left Front", leftFPower);
telemetry.addData("Right Back", rightBPower);
telemetry.addData("Left Back", leftBPower);
telemetry.addLine();
// telemetry.addData("", driveSpeed);
//telemetry.addData("Strafe Speed", strafeSpeed);
telemetry.update();
telemetry.update();
}
}
6 changes: 6 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/StrafeDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@ public void init() {
// init_loop() - Runs continuously until the driver hits play
@Override
public void init_loop() {
robot.previousGamepad1.copy(robot.currentGamepad1);
robot.currentGamepad1.copy(gamepad1);
}

// Start() - Runs ONCE when the driver presses play
Expand All @@ -44,6 +46,8 @@ public void start() {
// loop() - Runs continuously while the OpMode is active
@Override
public void loop() {
robot.previousGamepad1.copy(robot.currentGamepad1);
robot.currentGamepad1.copy(gamepad1);
double leftFPower;
double rightFPower;
double leftBPower;
Expand Down Expand Up @@ -76,5 +80,7 @@ public void loop() {
robot.leftRear.setPower(leftBPower);
robot.rightFront.setPower(rightFPower);
robot.rightRear.setPower(rightBPower);


}
}

0 comments on commit 1b3f057

Please sign in to comment.