Skip to content

Commit

Permalink
If all goes well, modify distance tester class to allow [selecting an…
Browse files Browse the repository at this point in the history 
… action and then executing it] instead of simply executing a pre-written command.
  • Loading branch information
@JimmyLi2020
JimmyLi2020 committed Feb 19, 2020
1 parent 57b738d commit e88545d
Show file tree
Hide file tree
Showing 5 changed files with 254 additions and 273 deletions.
51 changes: 40 additions & 11 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/DistanceTester.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,21 +5,50 @@
@Autonomous(name = "Distance Tester")
public class DistanceTester extends RobotController {

private final String[] actions = {"forward", "backward", "left", "right", "rotateRight", "rotateLeft"};

public void runOpMode() {
initMotors();
waitForStart();

moveForwardRaw(0.5);
sleep(500);
moveForwardRaw(0.0);
sleep(1500);
rotateLeftRaw(0.5);
sleep(850);
rotateLeftRaw(0.0);
sleep(1500);
moveForwardRaw(0.5);
sleep(1000);
moveForwardRaw(0.0);
int action = 0;
float distance = 0;

while (opModeIsActive()) {

telemetry.addData("Mode", "selecting action");
telemetry.addData("action", action);
telemetry.addData("distance", distance);
telemetry.update();

if (gamepad1.a) {
while (gamepad1.a) ;
if (action < 6)
action++;
}
if (gamepad1.y) {
while (gamepad1.y) ;
if (action > 0)
action--;
}
if (gamepad1.x) {
while (gamepad1.x) ;
if (distance < 10) ;
distance += 0.1;
}
if (gamepad1.b) {
while (gamepad1.b) ;
if (distance > 0.0)
distance -= 0.1;
}
if (gamepad1.right_bumper) {
while (gamepad1.right_bumper) ;
telemetry.update();
telemetry.addData("Mode", "doing action");
telemetry.update();
doAction(actions[action], distance);
}
}

stop();
}
Expand Down
51 changes: 11 additions & 40 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/RobotController.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.autonomous;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.util.ElapsedTime;
Expand All @@ -14,8 +13,6 @@ public abstract class RobotController extends LinearOpMode {
DcMotor motorBL;
DcMotor motorBR;

CRServo foundationClip;

final double calibFL = 1.00f;
final double calibFR = 1.00f;
final double calibBL = 1.00f;
Expand All @@ -30,24 +27,18 @@ public abstract class RobotController extends LinearOpMode {
final double DRIVE_SPEED = 0.6;
final double TURN_SPEED = 0.4;

private final String[] actions = {"forward", "backward", "left", "right", "rotateRight", "rotateLeft"}; //rotate = rotate right, negative rotate distance gives rotate left
private final String[] actions = {"forward", "backward", "left", "right", "rotateRight", "rotateLeft"};

public void initMotors() {
motorFL = hardwareMap.get(DcMotor.class, "frontLeft"); // frontLeft
motorFR = hardwareMap.get(DcMotor.class, "frontRight"); // frontRight
motorBL = hardwareMap.get(DcMotor.class, "backLeft"); // backLeft
motorBR = hardwareMap.get(DcMotor.class, "backRight"); // backRight

// this is for 2019-20 Skystone FTC where the robot drives backwards during autonomous.
// motorBR = hardwareMap.get(DcMotor.class, "frontLeft");
// motorBL = hardwareMap.get(DcMotor.class, "frontRight");
// motorFR = hardwareMap.get(DcMotor.class, "backLeft");
// motorFL = hardwareMap.get(DcMotor.class, "backRight");

motorFL.setDirection(DcMotorSimple.Direction.REVERSE);
motorFR.setDirection(DcMotorSimple.Direction.FORWARD);
motorBL.setDirection(DcMotorSimple.Direction.FORWARD);
motorBR.setDirection(DcMotorSimple.Direction.REVERSE);
motorBL.setDirection(DcMotorSimple.Direction.REVERSE);
motorBR.setDirection(DcMotorSimple.Direction.FORWARD);

motorFL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motorFR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
Expand All @@ -64,8 +55,6 @@ public void initMotors() {
motorBL.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
motorBR.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

foundationClip = hardwareMap.get(CRServo.class, "foundationClip");

}

public void moveForwardRaw(double power) {
Expand All @@ -82,7 +71,7 @@ public void rotateLeftRaw(double power) {
motorBR.setPower(calibBR * power);
}

public void straifLeftRaw(double power) {
public void strafeLeftRaw(double power) {
motorFL.setPower(calibFL * -power);
motorFR.setPower(calibFR * -power);
motorBL.setPower(calibBR * power);
Expand All @@ -104,10 +93,10 @@ public void doAction(String action, double distance) {
moveForward(-distance, distance * 2);
break;
case 2:
straifLeft(distance, distance * 2);
strafeLeft(distance, distance * 2);
break;
case 3:
straifLeft(-distance, distance * 2);
strafeLeft(-distance, distance * 2);
break;
case 4:
rotateLeft(distance, distance * 2);
Expand All @@ -128,30 +117,27 @@ public void rotateLeft(double distance, double timeout) {
encoderDrive(TURN_SPEED, -distance, distance, timeout, false);
}

public void straifLeft(double distance, double timeout) {
public void strafeLeft(double distance, double timeout) {
encoderDrive(DRIVE_SPEED, -distance, -distance, timeout, true);
}

public void encoderDrive(double speed,
double leftInches, double rightInches,
double timeoutS,
boolean straif) {
boolean strafe) {
int newFL;
int newFR;
int newBL;
int newBR;

int straifCoef = 1;
if (straif) {
straifCoef = -1;
}
int strafeCoef = strafe ? -1 : 1;

// Ensure that the opmode is still active
if (opModeIsActive()) {

// Determine new target position, and pass to motor controller
newFL = motorFL.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH * straifCoef);
newFR = motorFR.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH * straifCoef);
newFL = motorFL.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH * strafeCoef);
newFR = motorFR.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH * strafeCoef);
newBL = motorBL.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH);
newBR = motorBR.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);

Expand Down Expand Up @@ -189,21 +175,6 @@ public void encoderDrive(double speed,
motorFR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motorBL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motorBR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

sleep(1000);

}
}

public void clipFoundation() {
foundationClip.setPower(0.5);
sleep(500);
foundationClip.setPower(0.0);
}

public void releaseFoundation() {
foundationClip.setPower(-0.5);
sleep(500);
foundationClip.setPower(0.0);
}
}
11 changes: 0 additions & 11 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/TurnLeftToBridge.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,17 +10,6 @@ public void runOpMode() throws InterruptedException {
initMotors();
waitForStart();

moveForwardRaw(0.5);
sleep(500);
moveForwardRaw(0.0);
sleep(1500);
rotateLeftRaw(0.5);
sleep(850);
rotateLeftRaw(0.0);
sleep(1500);
moveForwardRaw(0.5);
sleep(1000);
moveForwardRaw(0.0);

stop();
}
Expand Down
15 changes: 3 additions & 12 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/TurnRightToBridge.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,25 +2,16 @@

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

@Autonomous(name = "TurnRightToBridge")
@Autonomous(name = "Forward Just A Bit")
public class TurnRightToBridge extends RobotController {
@Override
public void runOpMode() throws InterruptedException {
initMotors();
waitForStart();

moveForwardRaw(0.5);
sleep(500);
sleep(315);
moveForwardRaw(0.0);
sleep(1500);
rotateLeftRaw(-0.5);
sleep(850);
rotateLeftRaw(0.0);
sleep(1500);
moveForwardRaw(0.5);
sleep(1000);
moveForwardRaw(0.0);

stop();
sleep(297000);
}
}
Loading

0 comments on commit e88545d

Please sign in to comment.