Skip to content

Commit

Permalink
TeleOp for Meet #2
Browse files Browse the repository at this point in the history
  • Loading branch information
@iRoboRaider316
iRoboRaider316 committed Jan 25, 2018
1 parent 008be77 commit 9e6a497
Showing 1 changed file with 62 additions and 62 deletions.
124 changes: 62 additions & 62 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/caluper_teleop.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@

public class caluper_teleop extends OpMode {

public DcMotor lfDrive, rfDrive, lbDrive, rbDrive, liftMotor; //Left front drive, right front drive, left back drive, right back drive.
public Servo lServoArm, rServoArm, jewelArm; //lServoArm in port 5, rServoArm in port 4
public DcMotor lfDriveM, rfDriveM, lbDriveM, rbDriveM, liftM; //Left front drive, right front drive, left back drive, right back drive.
public Servo lArmS, rArmS, jewelArmS; //lServoArm in port 5, rServoArm in port 4

private double lServoArmInit = .73; //Glyph arms will initialize in the open position.
private double rServoArmInit = .1;
Expand All @@ -38,29 +38,29 @@ public class caluper_teleop extends OpMode {
ElapsedTime timer = new ElapsedTime();

public void init () {
lfDrive = hardwareMap.dcMotor.get("lfDrive"); //Left front drive, Hub 1, port 2
lfDrive.setPower(0);
lfDriveM = hardwareMap.dcMotor.get("lfDriveM"); //Left front drive, Hub 1, port 2
lfDriveM.setPower(0);

lbDrive = hardwareMap.dcMotor.get("lbDrive"); //Left back drive, Hub 1, port 3
lbDrive.setPower(0);
lbDriveM = hardwareMap.dcMotor.get("lbDriveM"); //Left back drive, Hub 1, port 3
lbDriveM.setPower(0);

rfDrive = hardwareMap.dcMotor.get("rfDrive"); //Right front drive, Hub 1, port 1
rfDrive.setPower(0);
rfDriveM = hardwareMap.dcMotor.get("rfDriveM"); //Right front drive, Hub 1, port 1
rfDriveM.setPower(0);

rbDrive = hardwareMap.dcMotor.get("rbDrive"); //Right back drive, Hub 1, port 0
rbDrive.setPower(0);
rbDriveM = hardwareMap.dcMotor.get("rbDriveM"); //Right back drive, Hub 1, port 0
rbDriveM.setPower(0);

liftMotor = hardwareMap.dcMotor.get("liftMotor"); //Lift motor, Hub 2, port 3
liftMotor.setPower(0);
liftM = hardwareMap.dcMotor.get("liftM"); //Lift motor, Hub 2, port 3
liftM.setPower(0);

lServoArm = hardwareMap.servo.get("lServoArm"); //Left servo arm, Hub 1, port 2
lServoArm.setPosition(lServoArmInit);
lArmS = hardwareMap.servo.get("lArmS"); //Left servo arm, Hub 1, port 2
lArmS.setPosition(lServoArmInit);

rServoArm = hardwareMap.servo.get("rServoArm"); //Right servo arm, Hub 2, port 1
rServoArm.setPosition(rServoArmInit);
rArmS = hardwareMap.servo.get("rArmS"); //Right servo arm, Hub 2, port 1
rArmS.setPosition(rServoArmInit);

jewelArm = hardwareMap.servo.get("jewelArm"); //Jewel Arm, Hub 2, port 3
jewelArm.setPosition(.5);
jewelArmS = hardwareMap.servo.get("jewelArmS"); //Jewel Arm, Hub 2, port 3
jewelArmS.setPosition(.5);
}

public void loop() {
Expand All @@ -79,22 +79,22 @@ else if(gamepad1.y) { //Hitting the "y" button will cut the drive train's po
///DRIVER CODE
switch(controlMode) {
case 1:
lfDrive.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //exponential scale algorithm
lbDrive.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //tank drive
rfDrive.setPower((gamepad1.right_stick_y * Math.abs(gamepad1.right_stick_y)) * speedFactor);
rbDrive.setPower((gamepad1.right_stick_y * Math.abs(gamepad1.right_stick_y)) * speedFactor);
lfDriveM.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //exponential scale algorithm
lbDriveM.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //tank drive
rfDriveM.setPower((gamepad1.right_stick_y * Math.abs(gamepad1.right_stick_y)) * speedFactor);
rbDriveM.setPower((gamepad1.right_stick_y * Math.abs(gamepad1.right_stick_y)) * speedFactor);
break;
case 2:
lfDrive.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //exponential scale algorithm
lbDrive.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //tank drive
rfDrive.setPower((gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor);
rbDrive.setPower((gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor);
lfDriveM.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //exponential scale algorithm
lbDriveM.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //tank drive
rfDriveM.setPower((gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor);
rbDriveM.setPower((gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor);
break;
default:
lfDrive.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //exponential scale algorithm
lbDrive.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //tank drive
rfDrive.setPower((gamepad1.right_stick_y * Math.abs(gamepad1.right_stick_y)) * speedFactor);
rbDrive.setPower((gamepad1.right_stick_y * Math.abs(gamepad1.right_stick_y)) * speedFactor);
lfDriveM.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //exponential scale algorithm
lbDriveM.setPower((-gamepad1.left_stick_y * Math.abs(gamepad1.left_stick_y)) * speedFactor); //tank drive
rfDriveM.setPower((gamepad1.right_stick_y * Math.abs(gamepad1.right_stick_y)) * speedFactor);
rbDriveM.setPower((gamepad1.right_stick_y * Math.abs(gamepad1.right_stick_y)) * speedFactor);
break;
}

Expand All @@ -114,12 +114,12 @@ else if(gamepad1.y) { //Hitting the "y" button will cut the drive train's po

if (gamepad1.right_stick_y == 0 && rightBackwardBrake == true) {
timer.reset();
rfDrive.setPower(-.3); //If the brake variable is true and the joystick resets to 0, the robot will reverse power briefly to cause an abrupt stop.
rbDrive.setPower(-.3);
rfDriveM.setPower(-.3); //If the brake variable is true and the joystick resets to 0, the robot will reverse power briefly to cause an abrupt stop.
rbDriveM.setPower(-.3);

if (timer.milliseconds() > 150) {
rfDrive.setPower(0);
rbDrive.setPower(0);
rfDriveM.setPower(0);
rbDriveM.setPower(0);
}
rightBackwardBrake = false; //As soon as the robot stops, reset the brake variable to false.
}
Expand All @@ -131,12 +131,12 @@ else if(gamepad1.y) { //Hitting the "y" button will cut the drive train's po

if (gamepad1.right_stick_y == 0 && rightForwardBrake == true) {
timer.reset();
rfDrive.setPower(.3);
rbDrive.setPower(.3);
rfDriveM.setPower(.3);
rbDriveM.setPower(.3);

if (timer.milliseconds() > 150) {
rfDrive.setPower(0);
rbDrive.setPower(0);
rfDriveM.setPower(0);
rbDriveM.setPower(0);
}
rightForwardBrake = false;
}
Expand All @@ -149,12 +149,12 @@ else if(gamepad1.y) { //Hitting the "y" button will cut the drive train's po

if (gamepad1.left_stick_y == 0 && leftBackwardBrake == true) {
timer.reset();
lfDrive.setPower(.3);
lbDrive.setPower(.3);
lfDriveM.setPower(.3);
lbDriveM.setPower(.3);

if (timer.milliseconds() > 150) {
lfDrive.setPower(0);
lbDrive.setPower(0);
lfDriveM.setPower(0);
lbDriveM.setPower(0);
}
leftBackwardBrake = false;
}
Expand All @@ -166,35 +166,35 @@ else if(gamepad1.y) { //Hitting the "y" button will cut the drive train's po

if (gamepad1.left_stick_y == 0 && leftForwardBrake == true) {
timer.reset();
lfDrive.setPower(-.3);
lbDrive.setPower(-.3);
lfDriveM.setPower(-.3);
lbDriveM.setPower(-.3);

if (timer.milliseconds() > 150) {
lfDrive.setPower(0);
lbDrive.setPower(0);
lfDriveM.setPower(0);
lbDriveM.setPower(0);
}
leftForwardBrake = false;
}

///OPERATOR CODE

if (gamepad2.dpad_up) {
liftMotor.setPower(1);
liftM.setPower(1);
}
else if (gamepad2.dpad_down) {
liftMotor.setPower(-1);
liftM.setPower(-1);
}
else {
liftMotor.setPower(0);
liftM.setPower(0);
}
if (gamepad2.dpad_up) {
liftMotor.setPower(1);
liftM.setPower(1);
}
else if (gamepad2.dpad_down) {
liftMotor.setPower(-1);
liftM.setPower(-1);
}
else {
liftMotor.setPower(0);
liftM.setPower(0);
}

/*
Expand All @@ -209,12 +209,12 @@ else if (gamepad2.dpad_down) {
*/

if (gamepad2.b) { //hitting the "b" button on Gamepad 2 will cause the glypher servos to grasp the glyph
lServoArm.setPosition(lServoArmGrasp);
rServoArm.setPosition(rServoArmGrasp);
lArmS.setPosition(lServoArmGrasp);
rArmS.setPosition(rServoArmGrasp);
}
if (gamepad2.x) { //hitting the "x" button on Gamepad 2 will cause the glypher servos to return to their original position
lServoArm.setPosition(lServoArmInit);
rServoArm.setPosition(rServoArmInit);
lArmS.setPosition(lServoArmInit);
rArmS.setPosition(rServoArmInit);
}

/*
Expand All @@ -225,21 +225,21 @@ else if (gamepad2.dpad_down) {
*/

if (gamepad2.y) { //hitting the "y" button on Gamepad 2 will cause the glypher servos to expand slightly larger than grasping the glyphs.
lServoArm.setPosition(lServoArmAlmostGrasp);
rServoArm.setPosition(rServoArmAlmostGrasp);
lArmS.setPosition(lServoArmAlmostGrasp);
rArmS.setPosition(rServoArmAlmostGrasp);
}

if (gamepad2.dpad_right) {
jewelArm.setPosition(1);
jewelArmS.setPosition(1);
}
else {
jewelArm.setPosition(.5);
jewelArmS.setPosition(.5);
}
if (gamepad2.dpad_left) {
jewelArm.setPosition(0);
jewelArmS.setPosition(0);
}
else {
jewelArm.setPosition(.5);
jewelArmS.setPosition(.5);
}

}
Expand Down

0 comments on commit 9e6a497

Please sign in to comment.