Skip to content

Commit

Permalink
[UNSTABLE] jem code
Browse files Browse the repository at this point in the history
  • Loading branch information
@123andrew12
123andrew12 committed Mar 9, 2018
1 parent a2dc24e commit ef3daf2
Show file tree
Hide file tree
Showing 3 changed files with 18 additions and 45 deletions.
14 changes: 7 additions & 7 deletions FtcRobotController/src/main/java/chawks/Autonomous/RedAllianceStone1withJewelforNextGen.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ public void runOpMode() throws InterruptedException {
);
telemetry.update();

/* navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "navx");
navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "navx");
gyro = (IntegratingGyroscope)navxMicro;
telemetry.log().add("Gyro Calibrating. Do Not Move!");

Expand All @@ -99,7 +99,7 @@ public void runOpMode() throws InterruptedException {
Thread.sleep(50);
}
telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
telemetry.clear(); telemetry.update();*/
telemetry.clear(); telemetry.update();


jewelDetector = new JewelDetector();
Expand Down Expand Up @@ -186,13 +186,13 @@ public void runOpMode() throws InterruptedException {
robot.VertGmServo.setPosition(ARM_START);
robot.LatGmServo.setPosition(PIVOT_START);
sleep(500);
/*navxTurn(90.0);
navxTurn(90.0);
telemetry.log().clear();
//Start Code after here
encoderDrive(.5,.5,-.5,.5,-.5,4);
//speed 5 is too fast, less than 7 dist is too short.
encoderDrive(.5,-6,-6,-6,-6,4);
navxTurn(0.0);*/
//navxTurn(0.0);


telemetry.addData("Path", "Complete");
Expand Down Expand Up @@ -280,7 +280,7 @@ void navxTurn(double target) {

String heading = formatAngle(angles.angleUnit, angles.firstAngle);

while (degrees > target + 2 || degrees < target - 2) {
while (degrees > target + 3 || degrees < target - 3) {
if (!opModeIsActive())
{
return;
Expand All @@ -289,12 +289,12 @@ void navxTurn(double target) {
angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);

heading = formatAngle(angles.angleUnit, angles.firstAngle);
if (degrees > target + 2) {
if (degrees > target + 3) {
robot.RBMotor.setPower(-BOT_SPEED);
robot.RFMotor.setPower(-BOT_SPEED);
robot.LFMotor.setPower(BOT_SPEED);
robot.LBMotor.setPower(BOT_SPEED);
} else if (degrees < target - 2) {
} else if (degrees < target - 3) {
robot.RBMotor.setPower(BOT_SPEED);
robot.RFMotor.setPower(BOT_SPEED);
robot.LFMotor.setPower(-BOT_SPEED);
Expand Down
41 changes: 7 additions & 34 deletions ...Controller/src/main/java/chawks/Autonomous/RedAllianceStone2withJewelFullIntegration.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,16 +22,16 @@
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;

import chawks.hardware.KBot;
import chawks.hardware.nextgen;

@Autonomous(name="RedAllianceStone2gemVuf", group="safezone")
@Disabled
//@Disabled
public class RedAllianceStone2withJewelFullIntegration extends LinearOpMode {
IntegratingGyroscope gyro;
NavxMicroNavigationSensor navxMicro;
public double degrees;
ElapsedTime timer = new ElapsedTime();
KBot robot = new KBot();
nextgen robot = new nextgen();
private ElapsedTime runtime = new ElapsedTime();
static final double BOT_SPEED = 0.2;
static final double COUNTS_PER_MOTOR_REV = 1440 ; // eg: TETRIX Motor Encoder
Expand Down Expand Up @@ -131,7 +131,7 @@ public void runOpMode() throws InterruptedException {



//ON KBOT WAAAAAAAAAAY TOO SPEEDY
//ON NextGen WAAAAAAAAAAY TOO SPEEDY
waitForStart();

//arm.setPosition(ARM_START);
Expand Down Expand Up @@ -211,6 +211,7 @@ public void runOpMode() throws InterruptedException {
//DogeCV camera disabled here
jewelDetector.disable();
sleep(250);
encoderDrive(.5,1,1,1,1,2);
//arm.setPosition(ARM_START);
//pivot.setPosition(PIVOT_START);
sleep(500);
Expand Down Expand Up @@ -264,47 +265,33 @@ public void runOpMode() throws InterruptedException {
}
if (glyphPlacement == 1) {
telemetry.update();
closeLift();
moveLiftAmount(1,500);
encoderDrive(.5,1,1,1,1,2);
navxTurn(90);
encoderDrive(.5,-3,-3,-3,-3,4);
navxTurn(180);
encoderDrive(.5,-3.25,-3.25,-3.25,-3.25,4);
navxTurn(90);
openLift();
encoderDrive(.5,-1,-1,-1,-1,4);
closeLift();
moveLiftAmount(-1,500);
encoderDrive(.5,1.125,1.125,1.125,1.125,4);
}
if (glyphPlacement == 2) {
telemetry.update();
closeLift();
moveLiftAmount(1,500);
navxTurn(90);
encoderDrive(.5,-3,-3,-3,-3,4);
navxTurn(180);
encoderDrive(.5,-3.25,-3.25,-3.25,-3.25,4);
navxTurn(90);
openLift();
encoderDrive(.5,-1,-1,-1,-1,4);
closeLift();
moveLiftAmount(-1,500);
encoderDrive(.5,1.125,1.125,1.125,1.125,4);
}
if (glyphPlacement == 3) {
telemetry.update();
closeLift();
moveLiftAmount(1,500);
navxTurn(90);
encoderDrive(.5,-3,-3,-3,-3,4);
navxTurn(180);
encoderDrive(.5,-3.25,-3.25,-3.25,-3.25,4);
navxTurn(90);
openLift();
encoderDrive(.5,-1,-1,-1,-1,4);
closeLift();
moveLiftAmount(-1,500);
encoderDrive(.5,1.125,1.125,1.125,1.125,4);
}

Expand Down Expand Up @@ -435,19 +422,5 @@ void navxTurn(double target) {
sleep(1500);
telemetry.log().clear();
}
public void moveLiftAmount (double power,long time) {
robot.LiftM.setPower(power);
sleep(time);
robot.LiftM.setPower(0);
}
public void closeLift () {
//was .7
robot.RGServo.setPosition(.7);
//was .3
robot.LGServo.setPosition(.3);
}
public void openLift () {
robot.RGServo.setPosition(.32);
robot.LGServo.setPosition(.68);
}

}
8 changes: 4 additions & 4 deletions FtcRobotController/src/main/java/chawks/Autonomous/RedAllianceStone2withJewelforNextGen.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ public void runOpMode() throws InterruptedException {
robot.RBMotor.getCurrentPosition()
);
telemetry.update();
/*navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "navx");
navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "navx");
gyro = (IntegratingGyroscope)navxMicro;
telemetry.log().add("Gyro Calibrating. Do Not Move!");

Expand All @@ -97,7 +97,7 @@ public void runOpMode() throws InterruptedException {
telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|");
telemetry.update();
Thread.sleep(50);
}*/
}
//telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
//telemetry.clear(); telemetry.update();
//DogeCV start here
Expand Down Expand Up @@ -188,12 +188,12 @@ public void runOpMode() throws InterruptedException {
robot.LatGmServo.setPosition(PIVOT_START);
sleep(500);
//end jewel arm sequence
/* navxTurn(90.0);
navxTurn(90.0);
//Start Code after here
encoderDrive(.75,-.5,.5,-.5,.5,4);
//speed 5 is too fast, less than 7 dist is too short.
encoderDrive(.5,-5,-5,-5,-5,4);
navxTurn(90.0); */
navxTurn(90.0);
//navx.turn(0.0, 0.2, robot);

telemetry.addData("Path", "Complete");
Expand Down

0 comments on commit ef3daf2

Please sign in to comment.