mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
ODOMETRY MIGHT WORK (more testing needed tho)
This commit is contained in:
aarav18
@@ -5,6 +5,8 @@
|
||||
package frc4388.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.utility.RobotTime;
|
||||
@@ -49,6 +51,9 @@ public class Robot extends TimedRobot {
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
|
||||
SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
|
||||
SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -63,6 +68,9 @@ public class Robot extends TimedRobot {
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {
|
||||
// SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
|
||||
// SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
|
||||
// SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -114,8 +122,8 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
m_robotContainer.getDriverController().updateInput();
|
||||
m_robotContainer.getOperatorController().updateInput();
|
||||
// m_robotContainer.getDriverController().updateInput();
|
||||
// m_robotContainer.getOperatorController().updateInput();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
package frc4388.robot;
|
||||
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
@@ -116,6 +117,9 @@ public class RobotContainer {
|
||||
return m_driverXbox;
|
||||
}
|
||||
|
||||
public Pose2d getOdometry() {
|
||||
return m_robotSwerveDrive.getOdometry();
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
@@ -61,32 +62,41 @@ public class RobotMap {
|
||||
rightBackSteerMotor.configFactoryDefault();
|
||||
rightBackWheelMotor.configFactoryDefault();
|
||||
|
||||
leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
leftFrontWheelMotor.setNeutralMode(NeutralMode.Coast);
|
||||
rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontWheelMotor.setNeutralMode(NeutralMode.Coast);
|
||||
leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackWheelMotor.setNeutralMode(NeutralMode.Coast);
|
||||
rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackWheelMotor.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
// config cancoder as remote encoder for swerve steer motors
|
||||
//leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
@@ -32,7 +32,7 @@ public class LED extends SubsystemBase {
|
||||
|
||||
@Override
|
||||
public void periodic(){
|
||||
SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
//SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -8,24 +8,20 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
|
||||
import edu.wpi.first.math.StateSpaceUtil;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
SwerveDriveKinematics m_kinematics;
|
||||
private WPI_TalonFX m_leftFrontSteerMotor;
|
||||
private WPI_TalonFX m_leftFrontWheelMotor;
|
||||
private WPI_TalonFX m_rightFrontSteerMotor;
|
||||
@@ -51,14 +47,14 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
// setSwerveGains();
|
||||
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
|
||||
private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
|
||||
|
||||
public SwerveModule[] modules;
|
||||
public WPI_PigeonIMU m_gyro;
|
||||
|
||||
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
|
||||
below are robot specific, and should be tuned. */
|
||||
private final SwerveDrivePoseEstimator m_poseEstimator;
|
||||
private SwerveDrivePoseEstimator m_poseEstimator;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
public boolean ignoreAngles;
|
||||
@@ -119,7 +115,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
double xSpeedMetersPerSecond = -speeds[0] * speedAdjust;
|
||||
double ySpeedMetersPerSecond = speeds[1] * speedAdjust;
|
||||
SwerveModuleState[] states =
|
||||
kinematics.toSwerveModuleStates(
|
||||
m_kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
|
||||
@@ -133,28 +129,18 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
System.err.println(m_gyro.getFusedHeading() +" aaa");
|
||||
//System.err.println(m_gyro.getFusedHeading() +" aaa");
|
||||
updateOdometry();
|
||||
// m_gyro.setFusedHeadingToCompass();
|
||||
// m_gyro.setYawToCompass();
|
||||
// m_gyro.getRotation2d();
|
||||
// System.out.println(m_gyro.getRotation2d());
|
||||
// System.out.println(modules[0].getState());
|
||||
// System.out.println(modules[1].getState());
|
||||
// System.out.println(modules[2].getState());
|
||||
// System.out.println(modules[3].getState());
|
||||
super.periodic();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get a "noisy" fake global pose reading.
|
||||
*
|
||||
* @param estimatedRobotPose The robot pose.
|
||||
*/
|
||||
// TEST FAKE CODE FROM EXAMPLE
|
||||
public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
|
||||
var rand =
|
||||
StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
|
||||
return new Pose2d(
|
||||
estimatedRobotPose.getX() + rand.get(0, 0),
|
||||
estimatedRobotPose.getY() + rand.get(1, 0),
|
||||
estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the distance between two given poses.
|
||||
* @param p1 The first pose.
|
||||
@@ -171,11 +157,22 @@ public class SwerveDrive extends SubsystemBase {
|
||||
* @param target_dist The target distance.
|
||||
* @return A scalar that multiplies your distance from the hub to get your target distance.
|
||||
*/
|
||||
public double getScalarForLine(double target_dist) {
|
||||
public Pose2d poseGivenDist(double target_dist) {
|
||||
Pose2d p1 = m_poseEstimator.getEstimatedPosition();
|
||||
Pose2d p2 = SwerveDriveConstants.HUB_POSE;
|
||||
|
||||
return target_dist/distBtwPoses(p1, p2);
|
||||
double scalar = target_dist/distBtwPoses(p1, p2);
|
||||
Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation());
|
||||
|
||||
return new_pose;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current pose of the robot.
|
||||
* @return Robot's current pose.
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
return m_poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
/** Updates the field relative position of the robot. */
|
||||
@@ -188,10 +185,9 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
m_poseEstimator.addVisionMeasurement(
|
||||
SwerveDrive.getEstimatedGlobalPose(
|
||||
m_poseEstimator.getEstimatedPosition()),
|
||||
Timer.getFPGATimestamp() - 0.1);
|
||||
// m_poseEstimator.addVisionMeasurement(
|
||||
// m_poseEstimator.getEstimatedPosition(),
|
||||
// Timer.getFPGATimestamp() - 0.1);
|
||||
}
|
||||
|
||||
public void highSpeed(boolean shift){
|
||||
|
||||
@@ -27,7 +27,7 @@ public class SwerveModule extends SubsystemBase {
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
private static double kEncoderTicksPerRotation = 4096;
|
||||
|
||||
private SwerveModuleState state;
|
||||
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
@@ -61,7 +61,7 @@ public class SwerveModule extends SubsystemBase {
|
||||
}
|
||||
|
||||
|
||||
public Rotation2d getAngle() {
|
||||
private Rotation2d getAngle() {
|
||||
// Note: This assumes the CANCoders are setup with the default feedback coefficient
|
||||
// and the sesnor value reports degrees.
|
||||
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
|
||||
@@ -74,8 +74,8 @@ public class SwerveModule extends SubsystemBase {
|
||||
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
|
||||
Rotation2d currentRotation = getAngle();
|
||||
|
||||
SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
//SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// Find the difference between our current rotational position + our new rotational position
|
||||
Rotation2d rotationDelta = state.angle.minus(currentRotation);
|
||||
@@ -100,7 +100,8 @@ public class SwerveModule extends SubsystemBase {
|
||||
* @return The current state of the module.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, new Rotation2d(canCoder.getPosition()));
|
||||
// return state;
|
||||
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -64,7 +64,7 @@ public class XboxController implements IHandController
|
||||
ret[1] = y / mag;
|
||||
}
|
||||
double [] to_smart_dashboard = {square_mag, x, y, ret[0], ret[1]};
|
||||
SmartDashboard.putNumberArray("Input", to_smart_dashboard);
|
||||
//SmartDashboard.putNumberArray("Input", to_smart_dashboard);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user