Skip to content

Commit

Permalink
2024-11-20 right autonomous odometry
Browse files Browse the repository at this point in the history
  • Loading branch information
@OviedoRobotics
OviedoRobotics committed Nov 21, 2024
1 parent 451f1f9 commit 1e7062d
Show file tree
Hide file tree
Showing 4 changed files with 215 additions and 50 deletions.
40 changes: 39 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,18 @@ private void unitTestOdometryDrive() {
} // unitTestOdometryDrive

/*--------------------------------------------------------------------------------------------*/
/* Autonomous Left: */
/* 1 Starting point */
/* 2 Place sample in upper bucket */
/* 3 Collect right neutral sample */
/* 4 Place sample in upper bucket */
/* 5 Collect center neutral sample */
/* 6 Place sample in upper bucket */
/* 7 Collect left neutral sample */
/* 8 Place sample in upper bucket */
/* 9 Level one ascent */
/*--------------------------------------------------------------------------------------------*/
// private void mainAutonomous(int scoreSamples) {
private void mainAutonomous() {

// Do we start with an initial delay?
Expand All @@ -145,7 +157,19 @@ private void mainAutonomous() {

// Score the preloaded specimen
parkSafeButNoPoints();

/*
// Score starting sample
scoreSample();
int samplesScored = 1;
while (samplesScored < scoreSamples) {
collectSample(samplesScored);
scoreSample();
samplesScored++;
}
level1Ascent();
*/
} // mainAutonomous

private void scoreSpecimenPreload() {
Expand Down Expand Up @@ -284,4 +308,18 @@ private void parkSafeButNoPoints() {

} // parkSafeButNoPoints

/*
private void scoreSample() {
} // scoreSample
private void collectSample(int samplesScored) {
} // scoreSample
private void level1Ascent() {
} // level1Ascent
*/

} /* AutonomousLeftRed */
81 changes: 63 additions & 18 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightBlue.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ public void runOpMode() throws InterruptedException {
//---------------------------------------------------------------------------------
// AUTONOMOUS ROUTINE: The following method is our main autonomous.
// Comment it out if running one of the unit tests above.
// mainAutonomous(5);
mainAutonomous();
//---------------------------------------------------------------------------------

Expand Down Expand Up @@ -133,6 +134,23 @@ private void unitTestOdometryDrive() {
} // unitTestOdometryDrive

/*--------------------------------------------------------------------------------------------*/
/* Autonomous Right: */
/* 1 Starting Point */
/* 2 Hang pre-load specimen at submersible */
/* 3 Herd left team sample */
/* 4 Collect specimen from observation zone (2nd preload specimen) */
/* 5 Hang specimen on high rung */
/* 6 Herd center team sample */
/* 7 Collect specimen from observation zone (from left herd) */
/* 8 Hang specimen on high rung */
/* 9 Herd right team sample */
/* 10 Collect specimen form observaiton zone (from center herd) */
/* 11 Hang specimen on high rung */
/* 12 Collect specimen from observation zone (from right herd) */
/* 13 Hang specimen on high rung */
/* 14 Park in observation zone */
/*--------------------------------------------------------------------------------------------*/
// private void mainAutonomous(int scoreSpecimens) {
private void mainAutonomous() {

// Do we start with an initial delay?
Expand All @@ -142,7 +160,18 @@ private void mainAutonomous() {

// Score the preloaded specimen
scoreSpecimenPreload();

/*
// Score starting specimen
scoreSpecimen(specimensScored);
int specimensScored = 1;
while (specimensScored < scoreSpecimens) {
herdSample(specimensScored);
collectSpecimen();
scoreSpecimen(specimensScored);
specimensScored++;
}
*/
// Score the preloaded specimen
parkInObservation();

Expand All @@ -154,19 +183,20 @@ private void scoreSpecimenPreload() {
telemetry.addData("Motion", "Move to submersible");
telemetry.update();
// Move away from field wall (viper slide motor will hit field wall if we tilt up too soon!)
// driveToPosition( 3.0, 0.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_THRU );
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_THRU );
driveToPosition( 3.0, 0.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU );
// gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_THRU );
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO1);
// driveToPosition( 6.0, 0.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_THRU );
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_THRU );
driveToPosition( 6.0, 0.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU );
// gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_THRU );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR1);
robot.wristServo.setPosition(robot.WRIST_SERVO_BAR1);
// driveToPosition( 9.0, 0.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_TO );
driveToPosition( 9.0, 0.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_TO );
// gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_TO );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR2);
robot.wristServo.setPosition(robot.WRIST_SERVO_BAR2);
// shift away from alliance partner
gyroDrive(DRIVE_SPEED_20, DRIVE_X, 6.0, 999.9, DRIVE_TO );
// approach submersible away from alliance partner
// gyroDrive(DRIVE_SPEED_20, DRIVE_X, 6.0, 999.9, DRIVE_TO );
driveToPosition( 9.0, -2.2, 0.0, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_TO );
robot.driveTrainMotorsZero();
do {
if( !opModeIsActive() ) break;
Expand All @@ -178,10 +208,10 @@ private void scoreSpecimenPreload() {
} // opModeIsActive

if( opModeIsActive() ) {
// driveToPosition( 22.0, 0.0, -45.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 23.0, 999.9, DRIVE_TO );
double startAngle = getAngle();
gyroTurn(TURN_SPEED_20, (startAngle - 45) ); // Turn CCW 45 degrees
driveToPosition( 32.0, -2.2, 45.0, DRIVE_SPEED_70, TURN_SPEED_50, DRIVE_TO );
// gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 23.0, 999.9, DRIVE_TO );
// double startAngle = getAngle();
// gyroTurn(TURN_SPEED_20, (startAngle - 45) ); // Turn CCW 45 degrees
robot.driveTrainMotorsZero();
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO1);
do {
Expand Down Expand Up @@ -253,15 +283,30 @@ private void scoreSpecimenPreload() {

} // scoreSpecimenPreload

/*
private void herdSample(int specimensScored) {
// TODO: FILL IN IMPLEMENTATION
}
private void collectSpecimen() {
// TODO: FILL IN IMPLEMENTATION
}
*/

private void parkInObservation() {
// Back up from submersible
gyroDrive(DRIVE_SPEED_40, DRIVE_Y, -23.0, 999.9, DRIVE_TO );
double startAngle = getAngle();
gyroTurn(TURN_SPEED_40, (startAngle + 135) ); // Turn CW 135 degrees
// gyroDrive(DRIVE_SPEED_40, DRIVE_Y, -23.0, 999.9, DRIVE_TO );
driveToPosition( 16.0, 14.0, 45.0, DRIVE_SPEED_50, TURN_SPEED_50, DRIVE_TO );
// double startAngle = getAngle();
// gyroTurn(TURN_SPEED_40, (startAngle + 135) ); // Turn CW 135 degrees
// Drive forward toward the wall
gyroDrive(DRIVE_SPEED_40, DRIVE_Y, 35.0, 999.9, DRIVE_TO );
driveToPosition( 12.0, 14.0, -91.0, DRIVE_SPEED_50, TURN_SPEED_50, DRIVE_TO );
driveToPosition( 12.0, 32.0, -91.0, DRIVE_SPEED_50, TURN_SPEED_50, DRIVE_TO );
// gyroDrive(DRIVE_SPEED_40, DRIVE_Y, 35.0, 999.9, DRIVE_TO );
// Strafe sideways toward the human player
gyroDrive(DRIVE_SPEED_20, DRIVE_X, 6.0, 999.9, DRIVE_TO );
// gyroDrive(DRIVE_SPEED_20, DRIVE_X, 6.0, 999.9, DRIVE_TO );
driveToPosition( 6.0, 32.0, -91.0, DRIVE_SPEED_30, TURN_SPEED_30, DRIVE_TO );

} // parkInObservation

} /* AutonomousRightBlue */
81 changes: 63 additions & 18 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightRed.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ public void runOpMode() throws InterruptedException {
//---------------------------------------------------------------------------------
// AUTONOMOUS ROUTINE: The following method is our main autonomous.
// Comment it out if running one of the unit tests above.
// mainAutonomous(5);
mainAutonomous();
//---------------------------------------------------------------------------------

Expand Down Expand Up @@ -133,6 +134,23 @@ private void unitTestOdometryDrive() {
} // unitTestOdometryDrive

/*--------------------------------------------------------------------------------------------*/
/* Autonomous Right: */
/* 1 Starting Point */
/* 2 Hang pre-load specimen at submersible */
/* 3 Herd left team sample */
/* 4 Collect specimen from observation zone (2nd preload specimen) */
/* 5 Hang specimen on high rung */
/* 6 Herd center team sample */
/* 7 Collect specimen from observation zone (from left herd) */
/* 8 Hang specimen on high rung */
/* 9 Herd right team sample */
/* 10 Collect specimen form observaiton zone (from center herd) */
/* 11 Hang specimen on high rung */
/* 12 Collect specimen from observation zone (from right herd) */
/* 13 Hang specimen on high rung */
/* 14 Park in observation zone */
/*--------------------------------------------------------------------------------------------*/
// private void mainAutonomous(int scoreSpecimens) {
private void mainAutonomous() {

// Do we start with an initial delay?
Expand All @@ -142,7 +160,18 @@ private void mainAutonomous() {

// Score the preloaded specimen
scoreSpecimenPreload();

/*
// Score starting specimen
scoreSpecimen(specimensScored);
int specimensScored = 1;
while (specimensScored < scoreSpecimens) {
herdSample(specimensScored);
collectSpecimen();
scoreSpecimen(specimensScored);
specimensScored++;
}
*/
// Score the preloaded specimen
parkInObservation();

Expand All @@ -154,19 +183,20 @@ private void scoreSpecimenPreload() {
telemetry.addData("Motion", "Move to submersible");
telemetry.update();
// Move away from field wall (viper slide motor will hit field wall if we tilt up too soon!)
// driveToPosition( 3.0, 0.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_THRU );
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_THRU );
driveToPosition( 3.0, 0.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU );
// gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_THRU );
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO1);
// driveToPosition( 6.0, 0.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_THRU );
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_THRU );
driveToPosition( 6.0, 0.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU );
// gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_THRU );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR1);
robot.wristServo.setPosition(robot.WRIST_SERVO_BAR1);
// driveToPosition( 9.0, 0.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_TO );
driveToPosition( 9.0, 0.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_TO );
// gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 3.0, 999.9, DRIVE_TO );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR2);
robot.wristServo.setPosition(robot.WRIST_SERVO_BAR2);
// shift away from alliance partner
gyroDrive(DRIVE_SPEED_20, DRIVE_X, 6.0, 999.9, DRIVE_TO );
// approach submersible away from alliance partner
// gyroDrive(DRIVE_SPEED_20, DRIVE_X, 6.0, 999.9, DRIVE_TO );
driveToPosition( 9.0, -2.2, 0.0, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_TO );
robot.driveTrainMotorsZero();
do {
if( !opModeIsActive() ) break;
Expand All @@ -178,10 +208,10 @@ private void scoreSpecimenPreload() {
} // opModeIsActive

if( opModeIsActive() ) {
// driveToPosition( 22.0, 0.0, -45.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 23.0, 999.9, DRIVE_TO );
double startAngle = getAngle();
gyroTurn(TURN_SPEED_20, (startAngle - 45) ); // Turn CCW 45 degrees
driveToPosition( 32.0, -2.2, 45.0, DRIVE_SPEED_70, TURN_SPEED_50, DRIVE_TO );
// gyroDrive(DRIVE_SPEED_20, DRIVE_Y, 23.0, 999.9, DRIVE_TO );
// double startAngle = getAngle();
// gyroTurn(TURN_SPEED_20, (startAngle - 45) ); // Turn CCW 45 degrees
robot.driveTrainMotorsZero();
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO1);
do {
Expand Down Expand Up @@ -253,15 +283,30 @@ private void scoreSpecimenPreload() {

} // scoreSpecimenPreload

/*
private void herdSample(int specimensScored) {
// TODO: FILL IN IMPLEMENTATION
}
private void collectSpecimen() {
// TODO: FILL IN IMPLEMENTATION
}
*/

private void parkInObservation() {
// Back up from submersible
gyroDrive(DRIVE_SPEED_40, DRIVE_Y, -23.0, 999.9, DRIVE_TO );
double startAngle = getAngle();
gyroTurn(TURN_SPEED_40, (startAngle + 135) ); // Turn CW 135 degrees
// gyroDrive(DRIVE_SPEED_40, DRIVE_Y, -23.0, 999.9, DRIVE_TO );
driveToPosition( 16.0, 14.0, 45.0, DRIVE_SPEED_50, TURN_SPEED_50, DRIVE_TO );
// double startAngle = getAngle();
// gyroTurn(TURN_SPEED_40, (startAngle + 135) ); // Turn CW 135 degrees
// Drive forward toward the wall
gyroDrive(DRIVE_SPEED_40, DRIVE_Y, 35.0, 999.9, DRIVE_TO );
driveToPosition( 12.0, 14.0, -91.0, DRIVE_SPEED_50, TURN_SPEED_50, DRIVE_TO );
driveToPosition( 12.0, 32.0, -91.0, DRIVE_SPEED_50, TURN_SPEED_50, DRIVE_TO );
// gyroDrive(DRIVE_SPEED_40, DRIVE_Y, 35.0, 999.9, DRIVE_TO );
// Strafe sideways toward the human player
gyroDrive(DRIVE_SPEED_20, DRIVE_X, 6.0, 999.9, DRIVE_TO );
// gyroDrive(DRIVE_SPEED_20, DRIVE_X, 6.0, 999.9, DRIVE_TO );
driveToPosition( 6.0, 32.0, -91.0, DRIVE_SPEED_30, TURN_SPEED_30, DRIVE_TO );

} // parkInObservation

} /* AutonomousRightRed */
Loading

0 comments on commit 1e7062d

Please sign in to comment.