Skip to content
This repository was archived by the owner on Apr 24, 2025. It is now read-only.

Commit a54780b

Browse files
mersenne-twistermersenne-twister
committed
auto change #1 test FIRST-Tech-Challenge#7 
1 parent 21a8afd commit a54780b

File tree

6 files changed

+215
-54
lines changed

6 files changed

+215
-54
lines changed

‎TeamCode/src/main/java/common/Arm.java‎

Lines changed: 23 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ public enum ARM_STATE { NONE, MOVE_HOME, MOVE_HIGH, MOVE_SAMPLE, MOVE_RUNG, ARM_
3333
private final double DRIVE_GEAR_REDUCTION = 26.9; // Gearing
3434
private final double WHEEL_DIAMETER_INCHES = (96 / 25.4); // 96 mm wheels converted to inches
3535

36-
private final double encoderDegree = 31.11; //encoder cts per degree 42.22
36+
private final double encoderDegree = 27.9; //encoder cts per degree 42.22
3737
private final double encoderInch = -1129 / 7.311;
3838

3939
public double lengthLeft = 0;
@@ -345,8 +345,8 @@ public void enablelArmRight (boolean enable) {
345345

346346

347347
public void run () {
348-
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
349-
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((rightElbow.getCurrentPosition() / encoderDegree) + 40));
348+
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
349+
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-rightElbow.getCurrentPosition() / encoderDegree + 40));
350350
opMode.telemetry.addData("lengthLeft", lengthLeft);
351351
opMode.telemetry.addData("lengthRight", lengthRight);
352352
//Logger.message("run");
@@ -696,11 +696,11 @@ public boolean control() {
696696
if (gamepad2.left_stick_y == 0 && rightElbowState == armStates.MOVE) {
697697
rightElbow.setPower(0);
698698
}
699-
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((rightElbow.getCurrentPosition() / encoderDegree) + 40));
699+
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-rightElbow.getCurrentPosition() / encoderDegree + 40));
700700
if (lengthRight >= 30) {
701701
rightElbow.setPower(-ELBOW_SPEED);
702702
rightElbowState = armStates.BARRIER;
703-
lengthRight = (rightArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((rightElbow.getCurrentPosition() / encoderDegree) + 40));
703+
lengthRight = (rightArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(rightElbow.getCurrentPosition() / encoderDegree + 40));
704704
} else if (gamepad2.left_stick_y == 0 || rightElbowState == armStates.BARRIER) {
705705
rightElbow.setPower(0);
706706
rightElbowState = armStates.OFF;
@@ -721,11 +721,11 @@ public boolean control() {
721721
if (gamepad2.left_stick_x == 0 && rightArmState == armStates.MOVE) {
722722
rightArm.setPower(0);
723723
}
724-
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((rightElbow.getCurrentPosition() / encoderDegree) + 40));
724+
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-rightElbow.getCurrentPosition() / encoderDegree + 40));
725725
if (lengthRight >= 30) {
726726
rightArm.setPower(ARM_SPEED);
727727
rightArmState = armStates.BARRIER;
728-
lengthRight = (rightArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((rightElbow.getCurrentPosition() / encoderDegree) + 40));
728+
lengthRight = (rightArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(-rightElbow.getCurrentPosition() / encoderDegree+ 40));
729729
} else if (gamepad2.left_stick_x == 0 || rightArmState == armStates.BARRIER) {
730730
rightArm.setPower(0);
731731
rightArmState = armStates.OFF;
@@ -747,11 +747,11 @@ public boolean control() {
747747
if (gamepad2.left_stick_y == 0 && leftElbowState == armStates.MOVE) {
748748
leftElbow.setPower(0);
749749
}
750-
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
750+
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
751751
if (lengthLeft >= 30) {
752752
leftElbow.setPower(-ELBOW_SPEED);
753753
leftElbowState = armStates.BARRIER;
754-
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
754+
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
755755
} else if (gamepad2.left_stick_y == 0 || leftElbowState == armStates.BARRIER){
756756
leftElbow.setPower(0);
757757
leftElbowState = armStates.OFF;
@@ -772,11 +772,11 @@ public boolean control() {
772772
if (gamepad2.left_stick_x == 0 && leftArmState == armStates.MOVE) {
773773
leftArm.setPower(0);
774774
}
775-
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
775+
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
776776
if (lengthLeft >= 30) {
777777
leftArm.setPower(ARM_SPEED);
778778
leftArmState = armStates.BARRIER;
779-
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
779+
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
780780
} else if (gamepad2.left_stick_x == 0 || leftArmState == armStates.BARRIER) {
781781
leftArm.setPower(0);
782782
leftArmState = armStates.OFF;
@@ -870,11 +870,11 @@ public boolean controlLeft() {
870870
if (gamepad1.left_stick_y == 0 && leftElbowState == armStates.MOVE) {
871871
leftElbow.setPower(0);
872872
}
873-
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
873+
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
874874
if (lengthLeft >= 30) {
875875
leftElbow.setPower(-ELBOW_SPEED);
876876
leftElbowState = armStates.BARRIER;
877-
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
877+
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
878878
} else if (gamepad1.left_stick_y == 0 || leftElbowState == armStates.BARRIER){
879879
leftElbow.setPower(0);
880880
leftElbowState = armStates.OFF;
@@ -895,11 +895,11 @@ public boolean controlLeft() {
895895
if (gamepad1.left_stick_x == 0 && leftArmState == armStates.MOVE) {
896896
leftArm.setPower(0);
897897
}
898-
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
898+
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
899899
if (lengthLeft >= 30) {
900900
leftArm.setPower(ARM_SPEED);
901901
leftArmState = armStates.BARRIER;
902-
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
902+
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
903903
} else if (gamepad1.left_stick_x == 0 || leftArmState == armStates.BARRIER) {
904904
leftArm.setPower(0);
905905
leftArmState = armStates.OFF;
@@ -998,11 +998,11 @@ public boolean controlLeft2() {
998998
if (gamepad2.left_stick_y == 0 && leftElbowState == armStates.MOVE) {
999999
leftElbow.setPower(0);
10001000
}
1001-
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
1001+
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
10021002
if (lengthLeft >= 30) {
10031003
leftElbow.setPower(-ELBOW_SPEED);
10041004
leftElbowState = armStates.BARRIER;
1005-
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
1005+
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
10061006
} else if (gamepad2.left_stick_y == 0 || leftElbowState == armStates.BARRIER){
10071007
leftElbow.setPower(0);
10081008
leftElbowState = armStates.OFF;
@@ -1023,11 +1023,11 @@ public boolean controlLeft2() {
10231023
if (gamepad2.left_stick_x == 0 && leftArmState == armStates.MOVE) {
10241024
leftArm.setPower(0);
10251025
}
1026-
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
1026+
lengthLeft = (leftArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
10271027
if (lengthLeft >= 30) {
10281028
leftArm.setPower(ARM_SPEED);
10291029
leftArmState = armStates.BARRIER;
1030-
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((leftElbow.getCurrentPosition() / encoderDegree) + 40));
1030+
lengthLeft = (leftArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(-leftElbow.getCurrentPosition() / encoderDegree + 40));
10311031
} else if (gamepad2.left_stick_x == 0 || leftArmState == armStates.BARRIER) {
10321032
leftArm.setPower(0);
10331033
leftArmState = armStates.OFF;
@@ -1157,11 +1157,11 @@ public boolean controlRight() {
11571157
if (gamepad2.left_stick_y == 0 && rightElbowState == armStates.MOVE) {
11581158
rightElbow.setPower(0);
11591159
}
1160-
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((rightElbow.getCurrentPosition() / encoderDegree) + 40));
1160+
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-rightElbow.getCurrentPosition() / encoderDegree + 40));
11611161
if (lengthRight >= 30) {
11621162
rightElbow.setPower(-ELBOW_SPEED);
11631163
rightElbowState = armStates.BARRIER;
1164-
lengthRight = (rightArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((rightElbow.getCurrentPosition() / encoderDegree) + 40));
1164+
lengthRight = (rightArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(-rightElbow.getCurrentPosition() / encoderDegree + 40));
11651165
} else if (gamepad2.left_stick_y == 0 || rightElbowState == armStates.BARRIER) {
11661166
rightElbow.setPower(0);
11671167
rightElbowState = armStates.OFF;
@@ -1182,11 +1182,11 @@ public boolean controlRight() {
11821182
if (gamepad2.left_stick_x == 0 && rightArmState == armStates.MOVE) {
11831183
rightArm.setPower(0);
11841184
}
1185-
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians((rightElbow.getCurrentPosition() / encoderDegree) + 40));
1185+
lengthRight = (rightArm.getCurrentPosition()/encoderInch + 10.5) * cos(Math.toRadians(-rightElbow.getCurrentPosition() / encoderDegree + 40));
11861186
if (lengthRight >= 30) {
11871187
rightArm.setPower(ARM_SPEED);
11881188
rightArmState = armStates.BARRIER;
1189-
lengthRight = (rightArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians((rightElbow.getCurrentPosition() / encoderDegree) + 40));
1189+
lengthRight = (rightArm.getCurrentPosition() / encoderInch + 10.5) * cos(Math.toRadians(-rightElbow.getCurrentPosition() / encoderDegree + 40));
11901190
} else if (gamepad2.left_stick_x == 0 || rightArmState == armStates.BARRIER) {
11911191
rightArm.setPower(0);
11921192
rightArmState = armStates.OFF;

‎TeamCode/src/main/java/common/Drive.java‎

Lines changed: 31 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -307,10 +307,10 @@ else if (speed > MAX_SPEED) {
307307

308308
double scale = (1 / max) * speed;
309309

310-
leftFrontPower *= scale;
310+
/*leftFrontPower *= scale;
311311
rightFrontPower *= scale;
312312
leftBackPower *= scale;
313-
rightBackPower *= scale;
313+
rightBackPower *= scale;*/
314314
}
315315

316316
// Send powers to the wheels.
@@ -358,15 +358,15 @@ public void moveRobot(DIRECTION direction, double speed) {
358358
leftBackSign = -1;
359359
rightBackSign = 1;
360360
} else if (direction == DIRECTION.TURN_LEFT) {
361-
leftFrontSign = -1;
362-
rightFrontSign = 1;
363-
leftBackSign = -1;
364-
rightBackSign = 1;
361+
leftFrontSign = 1;
362+
rightFrontSign = -1;
363+
leftBackSign = 1;
364+
rightBackSign = -1;
365365
} else if (direction == DIRECTION.TURN_RIGHT){
366-
leftFrontSign = 1;
367-
rightFrontSign = -1;
368-
leftBackSign = 1;
369-
rightBackSign = -1;
366+
leftFrontSign = -1;
367+
rightFrontSign = 1;
368+
leftBackSign = -1;
369+
rightBackSign = 1;
370370
}
371371

372372
// If the direction have changed get the encoder positions.
@@ -390,10 +390,15 @@ public void moveRobot(DIRECTION direction, double speed) {
390390
double leftBackAdjust = (maxPos - leftBackPos) * scale;
391391
double rightBackAdjust = (maxPos - rightBackPos) * scale;
392392

393-
double leftFrontPower = (speed + leftFrontAdjust) * leftFrontSign;
393+
/*double leftFrontPower = (speed + leftFrontAdjust) * leftFrontSign;
394394
double rightFrontPower = (speed + rightFrontAdjust) * rightFrontSign;
395395
double leftBackPower = (speed + leftBackAdjust) * leftBackSign;
396-
double rightBackPower = (speed + rightBackAdjust) * rightBackSign;
396+
double rightBackPower = (speed + rightBackAdjust) * rightBackSign;*/
397+
398+
double leftFrontPower = (speed) * leftFrontSign;
399+
double rightFrontPower = (speed) * rightFrontSign;
400+
double leftBackPower = (speed) * leftBackSign;
401+
double rightBackPower = (speed) * rightBackSign;
397402

398403
leftFrontDrive.setPower(leftFrontPower);
399404
rightFrontDrive.setPower(rightFrontPower);
@@ -500,20 +505,27 @@ public void moveDistance(DIRECTION direction, double speed, double inches, doubl
500505
double maxPos = Math.max(Math.max(Math.max(leftFrontPos, rightFrontPos), leftBackPos), rightBackPos);
501506

502507
double speedRange = Math.max(Math.abs(speed) - RAMP_MIN_SPEED, 0);
503-
double ramUp = (elapsedTime.milliseconds() / RAMP_TIME) * speedRange + RAMP_MIN_SPEED;
504-
double ramDown = (Math.pow((Math.abs(target) - maxPos), 2) / Math.pow(RAMP_DISTANCE, 2)) * speedRange + RAMP_MIN_SPEED;
505-
double rampPower = Math.min(Math.min(ramUp, ramDown), speed);
508+
//double ramUp = (elapsedTime.milliseconds() / RAMP_TIME) * speedRange + RAMP_MIN_SPEED;
509+
double ramUp = speedRange + RAMP_MIN_SPEED;
510+
//double ramDown = (Math.pow((Math.abs(target) - maxPos), 2) / Math.pow(RAMP_DISTANCE, 2)) * speedRange + RAMP_MIN_SPEED;
511+
//double rampPower = Math.min(Math.min(ramUp, ramDown), speed);
512+
double rampPower = Math.min(ramUp, speed);
506513

507514
double scale = .0015;
508515
double leftFrontAdjust = (maxPos - leftFrontPos) * scale;
509516
double rightFrontAdjust = (maxPos - rightFrontPos) * scale;
510517
double leftBackAdjust = (maxPos - leftBackPos) * scale;
511518
double rightBackAdjust = (maxPos - rightBackPos) * scale;
512519

513-
leftFrontDrive.setPower((rampPower + leftFrontAdjust) * leftFrontSign);
520+
/*leftFrontDrive.setPower((rampPower + leftFrontAdjust) * leftFrontSign);
514521
rightFrontDrive.setPower((rampPower + rightFrontAdjust) * rightFrontSign);
515522
leftBackDrive.setPower((rampPower + leftBackAdjust) * leftBackSign);
516-
rightBackDrive.setPower((rampPower + rightBackAdjust) * rightBackSign);
523+
rightBackDrive.setPower((rampPower + rightBackAdjust) * rightBackSign);*/
524+
525+
leftFrontDrive.setPower((rampPower) * leftFrontSign);
526+
rightFrontDrive.setPower((rampPower) * rightFrontSign);
527+
leftBackDrive.setPower((rampPower) * leftBackSign);
528+
rightBackDrive.setPower((rampPower) * rightBackSign);
517529

518530
for (DcMotor motor : motors)
519531
if (! motor.isBusy())
@@ -703,7 +715,7 @@ public void resetEncoders() {
703715
}
704716

705717
public double encoderTicksPerInch() {
706-
return (COUNTS_PER_INCH * DRIVE_FACTOR);
718+
return (25.017783857729079);
707719

708720
}
709721
public List<Double> getWheelPositions() {
@@ -771,7 +783,7 @@ public void strafeRight (double distance) {
771783

772784
public void turn(double degrees) {
773785

774-
double circumference = 2 * Math.PI * TURN_FACTOR;
786+
double circumference = 2 * Math.PI * 18.3003819;
775787
double inches = Math.abs(degrees) / 360 * circumference;
776788
if (degrees > 0)
777789
moveDistance(DIRECTION.TURN_LEFT, 1, inches, 0 );

‎TeamCode/src/main/java/main/MainTeleOp.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -108,7 +108,7 @@ else if (robot.arm.dropCommand())
108108
Logger.message("Changed split mode (to FOUR) at %f", runtime.seconds());
109109
mode = GamepadMode.FOUR;
110110
modeSplit = GamepadMode.FOUR;
111-
while (gamepad1.right_bumper) sleep(100);
111+
while (gamepad1.left_bumper) sleep(100);
112112
}
113113
} else if (mode == GamepadMode.FOUR) {
114114
robot.arm.specimenLeft.setPosition(gamepad2.right_trigger * 0.04 + 0.7);
@@ -125,7 +125,7 @@ else if (robot.arm.dropCommand())
125125
Logger.message("Changed split mode (to THREE) at %f", runtime.seconds());
126126
mode = GamepadMode.THREE;
127127
modeSplit = GamepadMode.THREE;
128-
while (gamepad1.right_bumper) sleep(100);
128+
while (gamepad1.left_bumper) sleep(100);
129129
}
130130
}
131131

‎TeamCode/src/main/java/main/SpecimenAuto.java‎

Lines changed: 13 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -46,30 +46,33 @@ public void runOpMode() {
4646
//robot.vision.enableCameraStream(false);
4747
//paused for new arm
4848
robot.arm.setSpecimenLeft(true);
49+
//sleep(14000);
50+
//robot.strafeRight(11);
4951
robot.arm.elbowMoveLeft(-1260);
5052
robot.arm.armMoveLeft(-1770);
51-
sleep(1000);
53+
//sleep(1000);
5254
robot.forward(30);
5355
robot.forwardSlow(4);
54-
sleep(200);
55-
robot.arm.elbowMoveLeft(-171);
56+
robot.back(2);
57+
//sleep(200);
58+
robot.arm.elbowMoveLeft(-450);
5659
sleep(1950);
5760
robot.arm.setSpecimenLeft(false);
5861
robot.back(10);
5962
robot.arm.elbowMoveLeft(0);
6063
robot.arm.armMoveLeft(0);
61-
robot.strafeRight(43);
64+
robot.strafeRight(41);
6265
robot.forward(36);
63-
robot.strafeRight(10);
66+
robot.strafeRight(13);
6467
robot.backFast(44);
6568
robot.forwardFast(48);
66-
robot.strafeRight(16);
67-
robot.turn(-15);
69+
robot.strafeRight(13);
70+
robot.turn(7);
6871
robot.backFast(44);
6972
robot.forwardFast(44);
70-
robot.strafeRight(10);
71-
robot.backFast(44);
72-
robot.forward(14);
73+
robot.strafeRight(19);
74+
robot.backFast(48);
75+
//robot.forward(14);
7376
/*robot.turn(180);
7477
robot.arm.elbowMove(Arm.ELBOW_SAMPLE);
7578
robot.arm.armMove(Arm.ARM_SAMPLE);

0 commit comments

Comments
 (0)