Skip to content

Commit

Permalink
2024-11-07 odometry drive
Browse files Browse the repository at this point in the history
  • Loading branch information
@OviedoRobotics
OviedoRobotics committed Nov 8, 2024
1 parent a84f640 commit cec119c
Show file tree
Hide file tree
Showing 5 changed files with 67 additions and 739 deletions.
36 changes: 21 additions & 15 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 @@ -14,6 +14,9 @@

import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
Expand All @@ -28,8 +31,7 @@

public abstract class AutonomousBase extends LinearOpMode {
/* Declare OpMode members. */
// HardwarePixelbot robot = new HardwarePixelbot(telemetry);
HardwareMinibot robot = new HardwareMinibot();
Hardware2025Bot robot = new Hardware2025Bot();

static final int DRIVE_TO = 1; // ACCURACY: tighter tolerances, and slows then stops at final position
static final int DRIVE_THRU = 2; // SPEED: looser tolerances, and leave motors running (ready for next command)
Expand Down Expand Up @@ -62,20 +64,20 @@ public abstract class AutonomousBase extends LinearOpMode {
static final double STRAFE_MULTIPLIER = 1.5;
static final double MIN_SPIN_RATE = 0.05; // Minimum power to turn the robot
static final double MIN_DRIVE_POW = 0.05; // Minimum speed to move the robot
// static final double MIN_DRIVE_MAGNITUDE = Math.sqrt(MIN_DRIVE_POW*MIN_DRIVE_POW+MIN_DRIVE_POW*MIN_DRIVE_POW);
static final double MIN_DRIVE_MAGNITUDE = Math.sqrt(MIN_DRIVE_POW*MIN_DRIVE_POW+MIN_DRIVE_POW*MIN_DRIVE_POW);

// NOTE: Initializing the odometry global X-Y and ANGLE to 0-0 and 0deg means the frame of reference for all movements is
// the starting positiong/orientation of the robot. An alternative is to make the bottom-left corner of the field the 0-0
// point, with 0deg pointing forward. That allows all absolute driveToPosition() commands to be an absolute x-y location
// on the field.
double robotGlobalXCoordinatePosition = 0.0; // in odometer counts
double robotGlobalYCoordinatePosition = 0.0;
double robotOrientationRadians = 0.0; // 0deg (straight forward)
double robotGlobalXCoordinatePosition = 0.0; // inches
double robotGlobalYCoordinatePosition = 0.0; // inches
double robotOrientationRadians = 0.0; // radians 0deg (straight forward)

// Odometry values corrected by external source, IE AprilTags
// FieldCoordinate robotGlobalCoordinateCorrectedPosition = new FieldCoordinate(0.0, 0.0, 0.0);

double autoXpos = 0.0; // Keeps track of our Autonomous X-Y position and Angle commands.
double autoXpos = 0.0; // Keeps track of our Autonomous X-Y position and Angle commands.
double autoYpos = 0.0; // (useful when a given value remains UNCHANGED from one
double autoAngle = 0.0; // movement to the next, or INCREMENTAL change from current location).

Expand Down Expand Up @@ -336,7 +338,11 @@ else if( gamepad1_cross_now && !gamepad1_cross_last ) {
/*---------------------------------------------------------------------------------*/
public void performEveryLoop() {
robot.readBulkData();
// globalCoordinatePositionUpdate();
robot.odom.update();
Pose2D pos = robot.odom.getPosition(); // x,y pos in inch; heading in degrees
robotGlobalXCoordinatePosition = pos.getY(DistanceUnit.INCH); // opposite x/y from goBilda pinpoint
robotGlobalYCoordinatePosition = pos.getX(DistanceUnit.INCH);
robotOrientationRadians = -pos.getHeading(AngleUnit.RADIANS); // 0deg (straight forward)
} // performEveryLoop

// Create a time stamped folder in
Expand Down Expand Up @@ -966,8 +972,8 @@ protected boolean driveToXY(double yTarget, double xTarget, double angleTarget,
int driveType) {
boolean reachedDestination = false;
// Not sure why, but the x and y are backwards
double xWorld = robotGlobalYCoordinatePosition / robot.COUNTS_PER_INCH2; // inches (backward! see notes)
double yWorld = robotGlobalXCoordinatePosition / robot.COUNTS_PER_INCH2; // inches
double xWorld = robotGlobalYCoordinatePosition; // inches (backward! see notes)
double yWorld = robotGlobalXCoordinatePosition; // inches
double xMovement, yMovement, turnMovement;
// Not sure why, but the x and y are backwards
double deltaX = yTarget - xWorld;
Expand Down Expand Up @@ -1029,10 +1035,10 @@ protected boolean driveToXY(double yTarget, double xTarget, double angleTarget,

// Convert from cm to inches
double errorMultiplier = 0.033;
// double speedMin = MIN_DRIVE_MAGNITUDE;
double speedMin = MIN_DRIVE_MAGNITUDE;
double allowedError = (driveType == DRIVE_THRU) ? 2.75 : 0.5;

return (driveToXY(yTarget, xTarget, angleTarget, 0.0 /*speedMin*/, speedMax, errorMultiplier,
return (driveToXY(yTarget, xTarget, angleTarget, speedMin, speedMax, errorMultiplier,
allowedError, driveType));
}

Expand Down Expand Up @@ -1125,9 +1131,9 @@ public void driveToPosition( double yTarget, double xTarget, double angleTarget,
public boolean moveToPosition( double xTarget, double yTarget, double angleTarget,
double speedMax, double turnMax, int driveType ) {
// Convert current robot X,Y position from encoder-counts to inches
double x_world = robotGlobalYCoordinatePosition / robot.COUNTS_PER_INCH2; // inches (X/Y backward! see notes)
double y_world = robotGlobalXCoordinatePosition / robot.COUNTS_PER_INCH2; // inches
double angle_world = robotOrientationRadians; // radians
double x_world = robotGlobalYCoordinatePosition; // inches (X/Y backward! see notes)
double y_world = robotGlobalXCoordinatePosition; // inches
double angle_world = robotOrientationRadians; // radians
// Compute distance and angle-offset to the target point
double distanceToPoint = Math.sqrt( Math.pow((xTarget - x_world),2.0) + Math.pow((yTarget - y_world),2.0) );
double distToPointAbs = Math.abs( distanceToPoint );
Expand Down
20 changes: 15 additions & 5 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 @@ -2,12 +2,17 @@
*/
package org.firstinspires.ftc.teamcode;

import static java.lang.Math.toDegrees;

import android.util.Size;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;

/**
* This program implements robot movement based on Gyro heading and encoder counts.
* It uses the Mecanumbot hardware class to define the drive on the robot.
Expand Down Expand Up @@ -82,13 +87,13 @@ public void runOpMode() throws InterruptedException {
// UNIT TEST: The following methods verify our basic robot actions.
// Comment them out when not being tested.
// testGyroDrive();
// unitTestOdometryDrive();
unitTestOdometryDrive();
//---------------------------------------------------------------------------------

//---------------------------------------------------------------------------------
// AUTONOMOUS ROUTINE: The following method is our main autonomous.
// Comment it out if running one of the unit tests above.
mainAutonomous();
// mainAutonomous();
//---------------------------------------------------------------------------------

telemetry.addData("Program", "Complete");
Expand All @@ -115,11 +120,16 @@ private void testGyroDrive() {
// TEST CODE: Verify odometry-based motion functions against a tape measure
private void unitTestOdometryDrive() {
// Drive forward 12"
driveToPosition( 12.0, 0.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_40, DRIVE_THRU );
driveToPosition( 12.0, 0.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_THRU );
// Strafe right 12"
driveToPosition( 12.0, 12.0, 0.0, DRIVE_SPEED_50, TURN_SPEED_40, DRIVE_THRU );
driveToPosition( 12.0, 12.0, 0.0, DRIVE_SPEED_20, TURN_SPEED_20, DRIVE_THRU );
// Turn 180 deg
driveToPosition( 12.0, 12.0, 179.9, DRIVE_SPEED_50, TURN_SPEED_40, 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) );
telemetry.update();
sleep( 7000 );
} // unitTestOdometryDrive

/*--------------------------------------------------------------------------------------------*/
Expand Down
32 changes: 12 additions & 20 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,21 +93,12 @@ public class Hardware2025Bot
public double wormTiltMotorSetPwr = 0.0; // requested power setting
public double wormTiltMotorPwr = 0.0; // current power setting

public double PAN_ANGLE_HW_MAX = 400.0; // absolute encoder angles at maximum rotation RIGHT
public double PAN_ANGLE_CYCLE_R = 133.5;
public double PAN_ANGLE_AUTO_M_R = 137.0; // scoring the autonomous pre-load cone (RIGHT MED)
public double PAN_ANGLE_AUTO_R = 117.5; // scoring the autonomous pre-load cone (RIGHT HIGH)
public double PAN_ANGLE_5STACK_L = 118.0;
public double PAN_ANGLE_COLLECT_R = 21.9;
public double PAN_ANGLE_CENTER = 0.0; // turret centered
public double PAN_ANGLE_COLLECT_L = -21.9;
public double PAN_ANGLE_5STACK_R = -103.0;
public double PAN_ANGLE_AUTO_L = -124.5; // scoring the autonomous pre-load cone (LEFT HIGH)
public double PAN_ANGLE_CYCLE_L = -133.5;
public double PAN_ANGLE_AUTO_M_L = -141.0; // scoring the autonomous pre-load cone (LEFT MED)
public double PAN_ANGLE_HW_MIN = -400.0; // absolute encoder angles at maximum rotation LEFT

public double TILT_ANGLE_HW_MAX = 4000.0; // encoder at maximum rotation UP/BACK
public double PAN_ANGLE_HW_MAX = 200.0; // encoder angles at maximum rotation RIGHT
public double PAN_ANGLE_HW_BASKET = 0.0; // encoder for rotation back to the basket for scoring
public double PAN_ANGLE_HW_MIN = -575.0; // encoder angles at maximum rotation LEFT

public double TILT_ANGLE_HW_MAX = 3675.0; // encoder at maximum rotation UP/BACK (horizontal = -200)
public double TILT_ANGLE_BASKET = 3675.0; // encoder at rotation back to the basket for scoring
public double TILT_ANGLE_HW_MIN = -2000.0; // encoder at maximum rotation DOWN/FWD

//====== Viper slide MOTOR (RUN_USING_ENCODER) =====
Expand All @@ -129,10 +120,11 @@ public class Hardware2025Bot
// 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_GRAB = 1230; // extend for collection from submersible
public int VIPER_EXTEND_GRAB = 1000; // extend for collection from submersible
public int VIPER_EXTEND_HOOK = 1038; // raised to where the specimen hook is above the high bar
public int VIPER_EXTEND_BASKET= 1482; // raised to basket-scoring height
public int VIPER_EXTEND_FULL = 3000; // fully extended (never exceed this count!)
public int VIPER_EXTEND_BASKET= 3000; // raised to basket-scoring height
public int VIPER_EXTEND_FULL1 = 2250; // extended 36" forward (max for 20"x42" limit) 2310 with overshoot
public int VIPER_EXTEND_FULL2 = 3010; // hardware fully extended (never exceed this count!)
PIDControllerLift liftPidController; // PID parameters for the lift motors
public double liftMotorPID_p = -0.100; // Raise p = proportional
public double liftMotorPID_i = 0.000; // Raise i = integral
Expand Down Expand Up @@ -532,8 +524,8 @@ public enum LiftStoreActivity {
public void startViperSlideExtension(int targetEncoderCount )
{
// Range-check the target
if( targetEncoderCount < VIPER_EXTEND_ZERO ) targetEncoderCount = VIPER_EXTEND_ZERO;
if( targetEncoderCount > VIPER_EXTEND_FULL ) targetEncoderCount = VIPER_EXTEND_FULL;
if( targetEncoderCount < VIPER_EXTEND_ZERO ) targetEncoderCount = VIPER_EXTEND_ZERO;
if( targetEncoderCount > VIPER_EXTEND_FULL2 ) targetEncoderCount = VIPER_EXTEND_FULL2;
// Configure target encoder count
viperMotor.setTargetPosition( targetEncoderCount );
// Enable RUN_TO_POSITION mode
Expand Down
38 changes: 19 additions & 19 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,6 @@ public abstract class Teleop extends LinearOpMode {
Gamepad.RumbleEffect rumblePixelBinSingle;
Gamepad.RumbleEffect rumblePixelBinDouble;


// sets unique behavior based on alliance
public abstract void setAllianceSpecificBehavior();

Expand Down Expand Up @@ -135,7 +134,7 @@ public void runOpMode() throws InterruptedException {
robot.odom.update();
Pose2D pos = robot.odom.getPosition(); // x,y pos in inch; heading in degrees
String posStr = String.format(Locale.US, "{X,Y: %.1f, %.1f in H: %.1f deg}",
pos.getX(DistanceUnit.INCH), pos.getY(DistanceUnit.INCH), pos.getHeading(AngleUnit.DEGREES));
pos.getX(DistanceUnit.INCH), pos.getY(DistanceUnit.INCH), -pos.getHeading(AngleUnit.DEGREES));
telemetry.addData("Position", posStr);
Pose2D vel = robot.odom.getVelocity(); // x,y velocities in inch/sec; heading in deg/sec
String velStr = String.format(Locale.US,"{X,Y: %.1f, %.1f in/sec, HVel: %.2f deg/sec}",
Expand Down Expand Up @@ -196,6 +195,7 @@ public void runOpMode() throws InterruptedException {
processPanControls();
processTiltControls();
ProcessViperLiftControls();
robot.processViperSlideExtension();
processCollectorControls();

// Compute current cycle time
Expand Down Expand Up @@ -563,7 +563,7 @@ void processTiltControls() {
boolean safeToManuallyLower = (robot.wormTiltMotorPos > robot.TILT_ANGLE_HW_MIN);
boolean safeToManuallyRaise = (robot.wormTiltMotorPos < robot.TILT_ANGLE_HW_MAX);
double gamepad2_right_stick = gamepad2.right_stick_y;
boolean manual_tilt_control = ( Math.abs(gamepad2_right_stick) > 0.10 );
boolean manual_tilt_control = ( Math.abs(gamepad2_right_stick) > 0.08 );

//===================================================================
// Check for an OFF-to-ON toggle of the gamepad1 CROSS button
Expand All @@ -580,15 +580,15 @@ else if( gamepad1_l_bumper_now && !gamepad1_l_bumper_last )
//===================================================================
else if( manual_tilt_control || tiltAngleTweaked) {
// Does user want to rotate turret DOWN (negative joystick input)
if( safeToManuallyLower && (gamepad2_right_stick < -0.10) ) {
double motorPower = 0.30 * gamepad2_right_stick; // NEGATIVE
robot.wormTiltMotor.setPower( motorPower ); // -3% to -30%
if( safeToManuallyLower && (gamepad2_right_stick < -0.08) ) {
double motorPower = 0.95 * gamepad2_right_stick; // NEGATIVE
robot.wormTiltMotor.setPower( motorPower ); // -8% to -95%
tiltAngleTweaked = true;
}
// Does user want to rotate turret UP (positive joystick input)
else if( safeToManuallyRaise && (gamepad2_right_stick > 0.10) ) {
double motorPower = 0.60 * gamepad2_right_stick; // POSITIVE
robot.wormTiltMotor.setPower( motorPower ); // +6% to +60%
else if( safeToManuallyRaise && (gamepad2_right_stick > 0.08) ) {
double motorPower = 0.95 * gamepad2_right_stick; // POSITIVE
robot.wormTiltMotor.setPower( motorPower ); // +8% to +95%
tiltAngleTweaked = true;
}
// No more input? Time to stop turret movement!
Expand All @@ -602,8 +602,8 @@ else if(tiltAngleTweaked) {

/*---------------------------------------------------------------------------------*/
void ProcessViperLiftControls() {
boolean safeToManuallyLower = (robot.viperMotorPos > robot.VIPER_EXTEND_ZERO);
boolean safeToManuallyRaise = (robot.viperMotorPos < robot.VIPER_EXTEND_FULL);
boolean safeToManuallyRetract = (robot.viperMotorPos > robot.VIPER_EXTEND_ZERO);
boolean safeToManuallyExtend = (robot.viperMotorPos < robot.VIPER_EXTEND_FULL1);
// Capture user inputs ONCE, in case they change during processing of this code
// or we want to scale them down
double gamepad2_left_trigger = gamepad2.left_trigger * 1.00;
Expand All @@ -614,30 +614,30 @@ void ProcessViperLiftControls() {
// Check for an OFF-to-ON toggle of the gamepad2 DPAD UP
if( gamepad2_dpad_up_now && !gamepad2_dpad_up_last)
{ // Move lift to HIGH-SCORING position
// robot.startLiftMove( robot.VIPER_EXTEND_BASKET );
robot.startLiftMove( robot.VIPER_EXTEND_BASKET );
}
// Check for an OFF-to-ON toggle of the gamepad2 DPAD RIGHT
else if( gamepad2_dpad_right_now && !gamepad2_dpad_right_last)
{ // Extend lift to the specimen-scoring hook-above-the-bar height
// robot.startLiftMove( robot.VIPER_EXTEND_HOOK );
}
// Check for an OFF-to-ON toggle of the gamepad2 DPAD LEFT
else if( gamepad2_dpad_left_now && !gamepad2_dpad_left_last)
// Check for an OFF-to-ON toggle of the gamepad2 DPAD DOWN
else if( gamepad2_dpad_down_now && !gamepad2_dpad_down_last)
{ // Move lift to STORED position
// robot.startLiftStore();
robot.startLiftMove( robot.VIPER_EXTEND_GRAB );
}
//===================================================================
else if( manual_lift_control || liftTweaked ) {
// Does user want to manually RAISE the lift?
if( safeToManuallyRaise && (gamepad2_right_trigger > 0.25) ) {
if( safeToManuallyExtend && (gamepad2_right_trigger > 0.25) ) {
// Do we need to terminate an auto movement?
robot.abortViperSlideExtension();
viperPower = gamepad2_right_trigger;
robot.viperMotor.setPower( viperPower ); // fixed power? (robot.VIPER_RAISE_POWER)
liftTweaked = true;
}
// Does user want to manually LOWER the lift?
else if( safeToManuallyLower && (gamepad2_left_trigger > 0.25) ) {
else if( safeToManuallyRetract && (gamepad2_left_trigger > 0.25) ) {
// Do we need to terminate an auto movement?
robot.abortViperSlideExtension();
viperPower = robot.VIPER_LOWER_POWER;
Expand Down Expand Up @@ -701,8 +701,8 @@ else if( gamepad2_r_bumper_now && !gamepad2_r_bumper_last)
geckoServoEjecting = false; // (we can't be doing this)
} // r_bumper

// Check for an OFF-to-ON toggle of the gamepad2 DPAD DOWN
else if( gamepad2_dpad_down_now && !gamepad2_dpad_down_last)
// Check for an OFF-to-ON toggle of the gamepad2 DPAD LEFT
else if( gamepad2_dpad_left_now && !gamepad2_dpad_left_last)
{ // Position for scoring a specimen on the submersible bar
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR);
robot.wristServo.setPosition(robot.WRIST_SERVO_BAR);
Expand Down
Loading

0 comments on commit cec119c

Please sign in to comment.