Skip to content

Commit

Permalink
2024-11-20 autonomous updates
Browse files Browse the repository at this point in the history
  • Loading branch information
@OviedoRobotics
OviedoRobotics committed Nov 22, 2024
1 parent 1e7062d commit 92c444b
Show file tree
Hide file tree
Showing 6 changed files with 107 additions and 105 deletions.
72 changes: 48 additions & 24 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftBlue.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@ private void unitTestOdometryDrive() {
// Strafe right 12"
driveToPosition( 12.0, 12.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
// Turn 180 deg
// driveToPosition( 12.0, 12.0, 90.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
driveToPosition( 12.0, 12.0, 90.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
// Report the final odometry position/orientation
telemetry.addData("Final", "x=%.1f, y=%.1f, %.1f deg",
robotGlobalXCoordinatePosition, robotGlobalYCoordinatePosition, toDegrees(robotOrientationRadians) );
Expand All @@ -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 @@ -142,9 +154,19 @@ private void mainAutonomous() {

// Score the preloaded specimen
scoreSpecimenPreload();

// Score the preloaded specimen
parkSafeButNoPoints();
/*
// Score starting sample
scoreSample();
int samplesScored = 1;
while (samplesScored < scoreSamples) {
collectSample(samplesScored);
scoreSample();
samplesScored++;
}
*/
// Park for 3pts (level 1 ascent)
level1Ascent();

} // mainAutonomous

Expand All @@ -154,19 +176,16 @@ 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 );
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 );
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 );
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, 3.0, 999.9, DRIVE_TO );
// small shift right in preparation for 45deg rotation (minimize shift toward alliance partner)
driveToPosition( 9.0, 3.0, 0.0, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_TO );
robot.driveTrainMotorsZero();
do {
if( !opModeIsActive() ) break;
Expand All @@ -178,10 +197,7 @@ 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, 6.5, 45.0, DRIVE_SPEED_70, TURN_SPEED_50, DRIVE_TO );
robot.driveTrainMotorsZero();
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO1);
do {
Expand Down Expand Up @@ -253,21 +269,29 @@ private void scoreSpecimenPreload() {

} // scoreSpecimenPreload

private void parkSafeButNoPoints() {
/*
private void scoreSample() {
// TODO: FILL IN IMPLEMENTATION
} // scoreSample
private void collectSample(int samplesScored) {
// TODO: FILL IN IMPLEMENTATION
} // collectSample
*/

private void level1Ascent() {
if( opModeIsActive() ) {
// Back up from submersible
// gyroDrive(DRIVE_SPEED_40, DRIVE_Y, -12.0, 999.9, DRIVE_TO );
double startAngle = getAngle();
gyroTurn(TURN_SPEED_40, (startAngle - 45) ); // Turn CCW 45 degrees
driveToPosition( 32.0, 6.0, 90.0, DRIVE_SPEED_50, TURN_SPEED_50, DRIVE_TO );
// Drive forward toward the wall
gyroDrive(DRIVE_SPEED_40, DRIVE_Y, 38.0, 999.9, DRIVE_THRU );
driveToPosition( 34.0, -27.0, 90.0, DRIVE_SPEED_50, TURN_SPEED_30, DRIVE_TO );
} // opModeIsActive

if( opModeIsActive() ) {
// Strafe towards submersible
gyroDrive(DRIVE_SPEED_40, DRIVE_X, 32.0, 999.9, DRIVE_THRU );
driveToPosition( 64.0, -27.0, 90.0, DRIVE_SPEED_70, TURN_SPEED_50, DRIVE_TO );
// Drive backward
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, -19, 999.9, DRIVE_TO );
driveToPosition( 64.0, -12.0, 90.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
} // opModeIsActive

if( opModeIsActive() ) {
Expand All @@ -282,6 +306,6 @@ private void parkSafeButNoPoints() {
} while( autoTiltMotorMoving() || autoViperMotorMoving() );
} // opModeIsActive

} // parkSafeButNoPoints
} // level1Ascent

} /* AutonomousLeftBlue */
62 changes: 24 additions & 38 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,9 +154,6 @@ private void mainAutonomous() {

// Score the preloaded specimen
scoreSpecimenPreload();

// Score the preloaded specimen
parkSafeButNoPoints();
/*
// Score starting sample
scoreSample();
Expand All @@ -167,9 +164,10 @@ private void mainAutonomous() {
scoreSample();
samplesScored++;
}
level1Ascent();
*/
// Park for 3pts (level 1 ascent)
level1Ascent();

} // mainAutonomous

private void scoreSpecimenPreload() {
Expand All @@ -178,19 +176,16 @@ 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 );
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 );
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 );
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, 3.0, 999.9, DRIVE_TO );
// small shift right in preparation for 45deg rotation (minimize shift toward alliance partner)
driveToPosition( 9.0, 3.0, 0.0, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_TO );
robot.driveTrainMotorsZero();
do {
if( !opModeIsActive() ) break;
Expand All @@ -202,10 +197,7 @@ 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, 6.5, 45.0, DRIVE_SPEED_70, TURN_SPEED_50, DRIVE_TO );
robot.driveTrainMotorsZero();
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO1);
do {
Expand Down Expand Up @@ -277,21 +269,29 @@ private void scoreSpecimenPreload() {

} // scoreSpecimenPreload

private void parkSafeButNoPoints() {
/*
private void scoreSample() {
// TODO: FILL IN IMPLEMENTATION
} // scoreSample
private void collectSample(int samplesScored) {
// TODO: FILL IN IMPLEMENTATION
} // collectSample
*/

private void level1Ascent() {
if( opModeIsActive() ) {
// Back up from submersible
// gyroDrive(DRIVE_SPEED_40, DRIVE_Y, -12.0, 999.9, DRIVE_TO );
double startAngle = getAngle();
gyroTurn(TURN_SPEED_40, (startAngle - 45) ); // Turn CCW 45 degrees
driveToPosition( 32.0, 6.0, 90.0, DRIVE_SPEED_50, TURN_SPEED_50, DRIVE_TO );
// Drive forward toward the wall
gyroDrive(DRIVE_SPEED_40, DRIVE_Y, 38.0, 999.9, DRIVE_THRU );
driveToPosition( 34.0, -27.0, 90.0, DRIVE_SPEED_50, TURN_SPEED_30, DRIVE_TO );
} // opModeIsActive

if( opModeIsActive() ) {
// Strafe towards submersible
gyroDrive(DRIVE_SPEED_40, DRIVE_X, 32.0, 999.9, DRIVE_THRU );
driveToPosition( 64.0, -27.0, 90.0, DRIVE_SPEED_70, TURN_SPEED_50, DRIVE_TO );
// Drive backward
gyroDrive(DRIVE_SPEED_20, DRIVE_Y, -19, 999.9, DRIVE_TO );
driveToPosition( 64.0, -12.0, 90.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
} // opModeIsActive

if( opModeIsActive() ) {
Expand All @@ -306,20 +306,6 @@ private void parkSafeButNoPoints() {
} while( autoTiltMotorMoving() || autoViperMotorMoving() );
} // opModeIsActive

} // parkSafeButNoPoints

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

} /* AutonomousLeftRed */
25 changes: 6 additions & 19 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 @@ -125,7 +125,7 @@ private void unitTestOdometryDrive() {
// Strafe right 12"
driveToPosition( 12.0, 12.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
// Turn 180 deg
// driveToPosition( 12.0, 12.0, 90.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
driveToPosition( 12.0, 12.0, 90.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
// Report the final odometry position/orientation
telemetry.addData("Final", "x=%.1f, y=%.1f, %.1f deg",
robotGlobalXCoordinatePosition, robotGlobalYCoordinatePosition, toDegrees(robotOrientationRadians) );
Expand Down Expand Up @@ -172,7 +172,7 @@ private void mainAutonomous() {
specimensScored++;
}
*/
// Score the preloaded specimen
// Park for 3pts (observation zone)
parkInObservation();

} // mainAutonomous
Expand All @@ -184,18 +184,14 @@ private void scoreSpecimenPreload() {
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_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_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_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);
// 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 {
Expand All @@ -209,9 +205,6 @@ private void scoreSpecimenPreload() {

if( opModeIsActive() ) {
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 @@ -290,22 +283,16 @@ private void herdSample(int specimensScored) {
private void collectSpecimen() {
// TODO: FILL IN IMPLEMENTATION
}
} // collectSpecimen
*/

private void parkInObservation() {
// Back up from submersible
// 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
// Rotate 90deg to face wall (protect collector from alliance partner damage)
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 );
driveToPosition( 6.0, 32.0, -91.0, DRIVE_SPEED_30, TURN_SPEED_30, DRIVE_TO );
// Park in far corner of observation zone
driveToPosition( 6.0, 32.0, -91.0, DRIVE_SPEED_50, TURN_SPEED_30, DRIVE_TO );

} // parkInObservation

Expand Down
25 changes: 6 additions & 19 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 @@ -125,7 +125,7 @@ private void unitTestOdometryDrive() {
// Strafe right 12"
driveToPosition( 12.0, 12.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
// Turn 180 deg
// driveToPosition( 12.0, 12.0, 90.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
driveToPosition( 12.0, 12.0, 90.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_TO );
// Report the final odometry position/orientation
telemetry.addData("Final", "x=%.1f, y=%.1f, %.1f deg",
robotGlobalXCoordinatePosition, robotGlobalYCoordinatePosition, toDegrees(robotOrientationRadians) );
Expand Down Expand Up @@ -172,7 +172,7 @@ private void mainAutonomous() {
specimensScored++;
}
*/
// Score the preloaded specimen
// Park for 3pts (observation zone)
parkInObservation();

} // mainAutonomous
Expand All @@ -184,18 +184,14 @@ private void scoreSpecimenPreload() {
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_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_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_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);
// 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 {
Expand All @@ -209,9 +205,6 @@ private void scoreSpecimenPreload() {

if( opModeIsActive() ) {
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 @@ -290,22 +283,16 @@ private void herdSample(int specimensScored) {
private void collectSpecimen() {
// TODO: FILL IN IMPLEMENTATION
}
} // collectSpecimen
*/

private void parkInObservation() {
// Back up from submersible
// 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
// Rotate 90deg to face wall (protect collector from alliance partner damage)
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 );
driveToPosition( 6.0, 32.0, -91.0, DRIVE_SPEED_30, TURN_SPEED_30, DRIVE_TO );
// Park in far corner of observation zone
driveToPosition( 6.0, 32.0, -91.0, DRIVE_SPEED_50, TURN_SPEED_30, DRIVE_TO );

} // parkInObservation

Expand Down
Loading

0 comments on commit 92c444b

Please sign in to comment.