Skip to content

Commit

Permalink
Created routine for scoring the 3 yellow samples in red left autonomous.
Browse files Browse the repository at this point in the history
  • Loading branch information
@OviedoRobotics
OviedoRobotics committed Dec 4, 2024
1 parent a6540b6 commit 033737a
Show file tree
Hide file tree
Showing 3 changed files with 108 additions and 23 deletions.
9 changes: 7 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -330,6 +330,12 @@ public void createAutoStorageFolder( boolean isRed, boolean isLeft ) {

/*---------------------------------------------------------------------------------*/
void autoViperMotorMoveToTarget(int targetEncoderCount )
{
autoViperMotorMoveToTarget(targetEncoderCount, 1.0);
} // autoViperMotorMoveToTarget

/*---------------------------------------------------------------------------------*/
void autoViperMotorMoveToTarget(int targetEncoderCount, double power)
{
// Configure target encoder count
robot.viperMotor.setTargetPosition( targetEncoderCount );
Expand All @@ -339,12 +345,11 @@ void autoViperMotorMoveToTarget(int targetEncoderCount )
boolean directionUpward = (targetEncoderCount > robot.viperMotorPos)? true : false;
// Set the power used to get there (NOTE: for RUN_TO_POSITION, always use a POSITIVE
// power setting, no matter which way the motor must rotate to achieve that target.
double motorPower = (directionUpward)? 1.0 : 0.5;
double motorPower = (directionUpward)? power : power / 2.0;
// Begin our timer and start the movement
autoViperMotorTimer.reset();
robot.viperMotor.setPower( motorPower );
} // autoViperMotorMoveToTarget

boolean autoViperMotorMoving() {
boolean viperMoving = true;
// Did the movement finish?
Expand Down
116 changes: 96 additions & 20 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 @@ -56,7 +56,7 @@ public void runOpMode() throws InterruptedException {

// Wait for the game to start (driver presses PLAY). While waiting, poll for options
redAlliance = true;
spikeSamples = 0; // until we get that code working...
spikeSamples = 3; // until we get that code working...
parkLocation = PARK_SUBMERSIBLE;

while (!isStarted()) {
Expand Down Expand Up @@ -158,23 +158,24 @@ private void mainAutonomous() {
}

// Score the preloaded specimen
if( !onlyPark && scorePreloadSpecimen ) {
scoreSpecimenPreload();
// if( !onlyPark && scorePreloadSpecimen ) {
// scoreSpecimenPreload();
// }

if( !onlyPark && (spikeSamples > 0) ) {
// Score starting sample
// scoreSample();
int samplesScored = 0;

while (samplesScored < spikeSamples) {
collectSample(samplesScored);
scoreSample();
samplesScored++;
}
}

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

// ensure motors are turned off even if we run out of time
robot.driveTrainMotorsZero();
Expand Down Expand Up @@ -285,15 +286,90 @@ private void scoreSpecimenPreload() {

} // scoreSpecimenPreload

/*
//************************************
// Score Sample
//************************************
private void scoreSample() {
// TODO: FILL IN IMPLEMENTATION
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 200 );
// update all our status
performEveryLoop();
} while( autoTiltMotorMoving() );
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO_PRE_DEG);
driveToPosition( 3.5, -47.5, -32.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO );
robot.startViperSlideExtension( robot.VIPER_EXTEND_BASKET );
robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_SAFE);
robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_AUTO_SCORE);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 50 );
// update all our status
performEveryLoop();
} while( autoTiltMotorMoving() );
robot.geckoServo.setPower( 1.0 );
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_BASKET_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 50 );
// update all our status
performEveryLoop();
} while( autoTiltMotorMoving() );
sleep(500);
robot.geckoServo.setPower( 0.0 );
robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB);
robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB);
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO_READY);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG);
} // scoreSample

//************************************
// Collect sample
//************************************
private void collectSample(int samplesScored) {
// TODO: FILL IN IMPLEMENTATION
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG);
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO_READY);

switch(samplesScored) {
case 0:
// Drive forward toward the wall
driveToPosition( 16.0, -38.0, 0.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO );
break;
case 1:
driveToPosition( 17.0, -46.5, 0.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO );
break;
case 2:
driveToPosition( 17.5, -53.0, 7.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO );
break;
default:
}
robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB);
robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_COLLECT_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 50 );
// update all our status
performEveryLoop();
} while( autoTiltMotorMoving() );
robot.geckoServo.setPower( -1.0 );
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO_COLLECT, 0.5);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 50 );
// update all our status
performEveryLoop();
} while( autoViperMotorMoving() );
robot.geckoServo.setPower( -0.30 );
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG);
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO_READY);
} // collectSample
*/


private void level1Ascent() {
if( opModeIsActive() ) {
Expand Down
6 changes: 5 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,7 @@ public class Hardware2025Bot
public final static double ENCODER_COUNTS_PER_DEG = 3896.0 / 94.5;
public final static double TILT_ANGLE_HW_MAX_DEG = 95.00; // encoder at maximum rotation UP/BACK (horizontal = -200)
public final static double TILT_ANGLE_BASKET_DEG = 95.00; // encoder at rotation back to the basket for scoring
public final static double TILT_ANGLE_AUTO_PRE_DEG = 92.00; // encoder at rotation back to the basket for scoring
public final static double TILT_ANGLE_ASCENT1_DEG = 93.80; // encoder at rotation back to the low bar for ascent level 1
public final static double TILT_ANGLE_RAISED_DEG = 54.50; // encoder at rotation back to the basket for scoring
public final static double TILT_ANGLE_HANG1_DEG = 40.10; // encoder when preparing for level 2 ascent
Expand All @@ -132,6 +133,7 @@ public class Hardware2025Bot
public final static double TILT_ANGLE_AUTO1_DEG = 54.80; // tilted up for autonomous specimen scoring (above bar)
public final static double TILT_ANGLE_AUTO2_DEG = 49.60; // tilted up for autonomous specimen scoring (clipped)
public final static double TILT_ANGLE_HW_MIN_DEG = 0.00; // encoder at maximum rotation DOWN/FWD
public final static double TILT_ANGLE_COLLECT_DEG = 2.00;

//====== Viper slide MOTOR (RUN_USING_ENCODER) =====
protected DcMotorEx viperMotor = null;
Expand All @@ -153,7 +155,8 @@ public class Hardware2025Bot
// Encoder counts for 435 RPM lift motors theoretical max 5.8 rev * 384.54 ticks/rev = 2230.3 counts
// Encoder counts for 312 RPM lift motors theoretical max ??? rev * 537.7 ticks/rev = ?? counts
public int VIPER_EXTEND_ZERO = 0; // fully retracted (may need to be adjustable??)
public int VIPER_EXTEND_AUTO = 482; // extend for collecting during auto
public int VIPER_EXTEND_AUTO_READY = 1000; // extend for collecting during auto
public int VIPER_EXTEND_AUTO_COLLECT = 2000; // extend for collecting during auto
public int VIPER_EXTEND_HANG1 = 2050; // extend to this to prepare for level 2 ascent
public int VIPER_EXTEND_HANG2 = 500; // retract to this extension during level 2 ascent
public int VIPER_EXTEND_GRAB = 1000; // extend for collection from submersible
Expand Down Expand Up @@ -193,6 +196,7 @@ public class Hardware2025Bot
final public static double WRIST_SERVO_SAFE = 0.340; // Safe orientation for driving
final public static double WRIST_SERVO_SAFE_ANGLE = 234.0;
final public static double WRIST_SERVO_GRAB = 0.860;
final public static double WRIST_SERVO_AUTO_SCORE = 0.600;
final public static double WRIST_SERVO_GRAB_ANGLE = 67.0;
final public static double WRIST_SERVO_RAISE = 0.570; // Safe orientation for driving
final public static double WRIST_SERVO_RAISE_ANGLE = 157.0;
Expand Down

0 comments on commit 033737a

Please sign in to comment.