From 3c0b92a4acf3774fb4ee4bead5062d6cdd1eb1ea Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 6 Mar 2020 18:46:45 -0700 Subject: [PATCH] Odometry Working haha --- .../java/frc4388/robot/RobotContainer.java | 16 +++++------ .../robot/commands/AutoPath1FromCenter.java | 12 ++++---- .../robot/commands/AutoPath2FromRight.java | 20 ++++++------- .../GotoCoordinatesFieldRelative.java | 28 +++++++++++++++++++ ...java => GotoCoordinatesRobotRelative.java} | 6 ++-- .../java/frc4388/robot/subsystems/Drive.java | 6 ++-- 6 files changed, 57 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java rename src/main/java/frc4388/robot/commands/{GotoCoordinates.java => GotoCoordinatesRobotRelative.java} (89%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 60b786e..4655a12 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -228,20 +228,20 @@ public class RobotContainer { Trajectory trajectory = getTrajectory(config); RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. - //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - + return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); - if (Constants.SELECTED_AUTO == 1) { + /*if (Constants.SELECTED_AUTO == 1) { return new SequentialCommandGroup(new Wait(m_robotDrive, 5, 0), new TurnDegrees(m_robotDrive, 45), new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive), new TurnDegrees(m_robotDrive, 315) ); - } + }*/ - return new InstantCommand(); + //return new InstantCommand(); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( @@ -257,14 +257,12 @@ public class RobotContainer { new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path List.of( - new Translation2d(10, 0) + new Translation2d(0, 50) ), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(20, 20, new Rotation2d(0)), + new Pose2d(50, 50, new Rotation2d(0)), // Pass config config); - // 10 = 20, 20 = 35, 30 = 53.5 - // (0,10) = (8,22) return exampleTrajectory; } diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java index 4ec080a..7af0b3f 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java +++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java @@ -29,15 +29,15 @@ public class AutoPath1FromCenter extends SequentialCommandGroup { addCommands( new Wait(m_drive, 0, 1), //shoot pre-loaded 3 balls - new GotoCoordinates(m_drive, m_pneumatics, 75, 44, -90), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 75, 44, -90), //Start Intake Ball 1 - new GotoCoordinates(m_drive, m_pneumatics, 0, 12), - new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 12), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 2 - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), - new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 3 - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), new Wait(m_drive, 0, 2) //Shoot 3 Balls ); diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java index 31ce8a3..5c9da9f 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java +++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java @@ -28,24 +28,24 @@ public class AutoPath2FromRight extends SequentialCommandGroup { m_pneumatics = subsystem2; addCommands( new Wait(m_drive, 0, 1), - new GotoCoordinates(m_drive, m_pneumatics, 0, 77), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 77), //Start Intake Ball 1 - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), - new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 2 - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), //Shoot 5 Balls - new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 6 (Ball 1 second round) - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), //Move to 7th Ball - new GotoCoordinates(m_drive, m_pneumatics, 86.7, -64.11, -180), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 86.7, -64.11, -180), //Move to 8th Ball - new GotoCoordinates(m_drive, m_pneumatics, -6.34, 15.31, 90), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, -6.34, 15.31, 90), //Move to 9th Ball - new GotoCoordinates(m_drive, m_pneumatics, 7.11, 24.41, 0), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 7.11, 24.41, 0), //Move to 10th Ball - new GotoCoordinates(m_drive, m_pneumatics, -6.34, 13.30), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, -6.34, 13.30), //Shoot 5 more Balls (Total 10 Ball Autonomous Path) new Wait(m_drive, 0, 2) ); diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java b/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java new file mode 100644 index 0000000..927d4b4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class GotoCoordinatesFieldRelative extends SequentialCommandGroup { + /** + * Creates a new GotoCoordinatesFieldRelative. + */ + public GotoCoordinatesFieldRelative(Drive susbsytem, Pneumatics subsystem2, double xTarget, double yTarget) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + addCommands( + + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java similarity index 89% rename from src/main/java/frc4388/robot/commands/GotoCoordinates.java rename to src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java index 6ca3deb..af8e29a 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java @@ -15,7 +15,7 @@ import frc4388.robot.subsystems.Pneumatics; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class GotoCoordinates extends SequentialCommandGroup { +public class GotoCoordinatesRobotRelative extends SequentialCommandGroup { Drive m_drive; Pneumatics m_pneumatics; @@ -28,7 +28,7 @@ public class GotoCoordinates extends SequentialCommandGroup { /** * Creates a new GotoPosition. */ - public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { + public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); m_drive = subsystem; @@ -49,7 +49,7 @@ public class GotoCoordinates extends SequentialCommandGroup { new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); } - public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { + public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { m_drive = subsystem; m_pneumatics = subsystem2; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 059d3c6..0613050 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -442,8 +442,8 @@ public class Drive extends SubsystemBase { // m_currentAngleYaw-(360), // m_currentAngleYaw+(360)); //double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; - double moveVelLeft = inchesToTicks(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; - double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; + double moveVelLeft = inchesToTicks(leftSpeed)/DriveConstants.SECONDS_TO_TICK_TIME; + double moveVelRight = inchesToTicks(rightSpeed)/DriveConstants.SECONDS_TO_TICK_TIME; //SmartDashboard.putNumber("Move Vel Left", moveVelLeft); //SmartDashboard.putNumber("Move Vel Right", moveVelRight); @@ -783,7 +783,7 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - //SmartDashboard.putString("Odometry Values Meters", getPose().toString()); + SmartDashboard.putString("Odometry Values Meters", getPose().toString()); //SmartDashboard.putNumber("Odometry Heading", getHeading()); //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);