Skip to content

Commit

Permalink
Not perfect, but close.
Browse files Browse the repository at this point in the history
  • Loading branch information
OviedoRobotics committed Dec 6, 2024
1 parent 033737a commit 66c45dc
Show file tree
Hide file tree
Showing 4 changed files with 452 additions and 159 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -276,7 +276,7 @@ else if( gamepad1_cross_now && !gamepad1_cross_last ) {
telemetry.addData("Park Location","%s %s", parkLocationStr[parkLocation],
((initMenuSelected==5)? "<-":" "));
telemetry.addData("spike specimens", "%d %s",spikeSamples,((initMenuSelected==6)? "<-":" ") );
telemetry.addData(">","version 100" );
telemetry.addData(">","version 102" );
telemetry.update();
} // processAutonomousInitMenu

Expand Down Expand Up @@ -677,7 +677,7 @@ public boolean strafeToWall(boolean leftWall, double maxSpeed, int distanceFromW
rotatePower = angleError * angleErrorMult;
drivePower = distanceError * distanceErrorMult;
drivePower = leftWall ? drivePower : -drivePower;
drivePower = Math.copySign(Math.min(Math.max(Math.abs(drivePower), robot.MIN_STRAFE_POW), maxPower), drivePower);
drivePower = Math.copySign(Math.min(Math.max(Math.abs(drivePower), Hardware2025Bot.MIN_STRAFE_POW), maxPower), drivePower);
fl = -drivePower + rotatePower;
fr = drivePower - rotatePower;
bl = drivePower + rotatePower;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -158,11 +158,16 @@ private void mainAutonomous() {
}

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

if( !onlyPark && (spikeSamples > 0) ) {
driveToPosition( 16.0, -19.0, 0.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_THRU );
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG);
autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO_READY);
robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB);
robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB);
// Score starting sample
// scoreSample();
int samplesScored = 0;
Expand All @@ -173,7 +178,6 @@ private void mainAutonomous() {
samplesScored++;
}
}
sleep(2000);
// Park for 3pts (level 1 ascent)
// level1Ascent();

Expand All @@ -189,41 +193,29 @@ private void scoreSpecimenPreload() {
telemetry.update();
// Move away from field wall (viper slide motor will hit field wall if we tilt up too soon!)
pos_y=3.0; pos_x=0.0; pos_angle=0.0; // start at this absolute location
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU );
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO1_DEG);
pos_y += 3.0;
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_THRU );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR1);
robot.wristServo.setPosition(robot.WRIST_SERVO_BAR1);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO1_DEG);
pos_y += 3.0;
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_THRU );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR2);
robot.wristServo.setPosition(robot.WRIST_SERVO_BAR2);
// small shift right in preparation for 45deg rotation (minimize shift toward alliance partner)
robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_BAR2);
robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_BAR2);
pos_y += 6.0;
pos_x += 3.0;
// small shift right in preparation for 45deg rotation (minimize shift toward alliance partner)
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_TO );
robot.driveTrainMotorsZero();
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 150 );
// update all our status
performEveryLoop();
} while( autoTiltMotorMoving() );
autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO1);
} // opModeIsActive

if( opModeIsActive() ) {
pos_y += 23.0; pos_x += 3.5; pos_angle = 45.0;
pos_y += 23.0; pos_x += 5.0; pos_angle = 45.0;
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_50, DRIVE_TO );
robot.driveTrainMotorsZero();
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO1);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 200 );
sleep( 50 );
// update all our status
performEveryLoop();
} while( autoViperMotorMoving() );
} while( autoViperMotorMoving() || autoTiltMotorMoving());
} // opModeIsActive

// Rotate lift down to get specimen close to bar
Expand All @@ -233,19 +225,19 @@ private void scoreSpecimenPreload() {
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 150 );
sleep( 50 );
// update all our status
performEveryLoop();
} while( autoTiltMotorMoving() );
} // opModeIsActive

// Retract lift to clip the specimen on the bar
if( opModeIsActive() ) {
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_AUTO2);
autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO2);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 200 );
sleep( 50 );
// update all our status
performEveryLoop();
} while( autoViperMotorMoving() );
Expand All @@ -258,33 +250,63 @@ private void scoreSpecimenPreload() {
robot.geckoServo.setPower(0.0); // stop
} // opModeIsActive

// Retract the arm for parking
// Setup the arm for scoring samples
if( opModeIsActive() ) {
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_ZERO);
autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_SECURE);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 200 );
sleep( 50 );
// update all our status
performEveryLoop();
} while( autoViperMotorMoving() );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_INIT);
robot.wristServo.setPosition(robot.WRIST_SERVO_INIT);
robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB);
robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB);
} // opModeIsActive
} // scoreSpecimenPreload

// Lower the arm for parking
if( opModeIsActive() ) {
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 150 );
// update all our status
performEveryLoop();
} while( autoTiltMotorMoving() );
} // opModeIsActive
//************************************
// Collect sample
//************************************
private void collectSample(int samplesScored) {
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG);
autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO_READY);
robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_GRAB);
robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_GRAB);

} // scoreSpecimenPreload
switch(samplesScored) {
case 0:
// Drive forward toward the wall
driveToPosition( 16.0, -37.0, 0.0, DRIVE_SPEED_90, TURN_SPEED_50, DRIVE_TO );
break;
case 1:
driveToPosition( 17.0, -46.0, 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:
}
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( Hardware2025Bot.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() );
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG);
autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO_READY);
} // collectSample

//************************************
// Score Sample
Expand All @@ -293,13 +315,13 @@ private void scoreSample() {
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
sleep( 200 );
sleep( 50 );
// 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.startViperSlideExtension( Hardware2025Bot.VIPER_EXTEND_BASKET );
robot.elbowServo.setPosition(Hardware2025Bot.ELBOW_SERVO_SAFE);
robot.wristServo.setPosition(Hardware2025Bot.WRIST_SERVO_AUTO_SCORE);
do {
Expand All @@ -322,55 +344,10 @@ private void scoreSample() {
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);
autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_AUTO_READY);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_DRIVE_DEG);
} // scoreSample

//************************************
// Collect sample
//************************************
private void collectSample(int samplesScored) {
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() ) {
// Back up from submersible
Expand All @@ -387,7 +364,7 @@ private void level1Ascent() {
} // opModeIsActive

if( opModeIsActive() ) {
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB);
autoViperMotorMoveToTarget( Hardware2025Bot.VIPER_EXTEND_GRAB);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ASCENT1_DEG);
timeDriveStraight(-DRIVE_SPEED_20,3000);
do {
Expand Down
Loading

0 comments on commit 66c45dc

Please sign in to comment.