summaryrefslogtreecommitdiff
path: root/src/main/java/frc/robot
diff options
context:
space:
mode:
Diffstat (limited to 'src/main/java/frc/robot')
-rw-r--r--src/main/java/frc/robot/Constants.java6
-rw-r--r--src/main/java/frc/robot/Robot.java16
-rw-r--r--src/main/java/frc/robot/RobotContainer.java21
-rw-r--r--src/main/java/frc/robot/subsystems/Drivetrain.java39
-rw-r--r--src/main/java/frc/robot/util/IMUGyro.java10
5 files changed, 60 insertions, 32 deletions
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 6d471db..98824ae 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -16,10 +16,10 @@ public class Constants
public static final double DriveMaxSpeed = 5; // m/s
public static final double DriveMaxAngular = Math.PI; // rad/s?
- public static final double WheelRadius = 0.0254; // in meters
- public static final double TrackWidth = 0.74; // in meters
+ public static final double WheelDiameter = 0.1524; // in meters
+ public static final double TrackWidth = 0.685; // in meters
- public static final double EncoderResolution = 4096;
+ public static final double EncoderResolution = 4096 / 24;
public static final IMUGyro gyro = new IMUGyro(new I2C(Port.kOnboard, 0x68));
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index b68462c..8a3f9b4 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,6 +4,10 @@
package frc.robot;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -54,8 +58,18 @@ public class Robot extends TimedRobot {
}
}
+ GenericHID controller = Constants.controller;
+ // CANSparkMax shooter_top = new CANSparkMax(21, MotorType.kBrushless);
+ // CANSparkMax shooter_bottom = new CANSparkMax(22, MotorType.kBrushless);
+ CANSparkMax intakeL = new CANSparkMax(31, MotorType.kBrushless);
+ CANSparkMax intakeR = new CANSparkMax(32, MotorType.kBrushless);
+
@Override
- public void teleopPeriodic() {}
+ public void teleopPeriodic()
+ {
+ intakeL.set(controller.getRawAxis(1));
+ intakeR.set(controller.getRawAxis(3));
+ }
@Override
public void teleopExit() {}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d2cede4..64ef818 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -29,15 +29,15 @@ public class RobotContainer
private void configureBindings()
{
- m_drive.setDefaultCommand(m_drive.run(() ->
- // m_drive.arcade(
- // deadzone(-1, 1, m_controller.getRawAxis(1) * Constants.DriveMaxSpeed),
- // deadzone(-1, 1, m_controller.getRawAxis(0) * Constants.DriveMaxAngular)
- // )));
- m_drive.tank(
- deadzone(-0.09, 0.09, m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
- deadzone(-0.09, 0.09, m_controller.getRawAxis(3)) * Constants.DriveMaxSpeed
- )));
+ // m_drive.setDefaultCommand(m_drive.run(() ->
+ // // m_drive.arcade(
+ // // deadzone(-1, 1, m_controller.getRawAxis(1) * Constants.DriveMaxSpeed),
+ // // deadzone(-1, 1, m_controller.getRawAxis(0) * Constants.DriveMaxAngular)
+ // // )));
+ // m_drive.tank(
+ // deadzone(-0.09, 0.09, m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
+ // deadzone(-0.09, 0.09, m_controller.getRawAxis(3)) * Constants.DriveMaxSpeed
+ // )));
new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> {
Pose2d pose = m_drive.pose2d();
@@ -48,9 +48,6 @@ public class RobotContainer
System.out.println("Reset");
m_drive.reset();
}));
- // new Trigger(() -> m_controller.getRawButton(3)).onTrue(Commands.run(() -> {
- // m_drive.tank(5, 5);
- // }));
}
public Command getAutonomousCommand()
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 80ed06e..a7fe75e 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -9,8 +9,13 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.proto.Trajectory;
import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
import frc.robot.Constants;
import frc.robot.util.IMUGyro;
@@ -26,7 +31,7 @@ public class Drivetrain extends SubsystemBase
private Encoder m_encoderL = Constants.encoderL;
private Encoder m_encoderR = Constants.encoderR;
- private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1.5, 2.82, 0.47);
+ private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1.1, 3.16, 0.17);
private final PIDController m_pidL = new PIDController(1, 0, 0);
private final PIDController m_pidR = new PIDController(1, 0, 0);
@@ -43,10 +48,12 @@ public class Drivetrain extends SubsystemBase
m_motorR_follower.follow(m_motorR_leader);
m_motorL_follower.follow(m_motorL_leader);
+
m_motorR_leader.setInverted(true);
+ m_encoderL.setReverseDirection(true);
- m_encoderL.setDistancePerPulse(-2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
- m_encoderR.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
+ m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
+ m_encoderR.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
m_encoderL.reset();
m_encoderR.reset();
@@ -72,21 +79,24 @@ public class Drivetrain extends SubsystemBase
public void speed(DifferentialDriveWheelSpeeds speeds)
{
- m_motorL_leader.setVoltage(
+ voltage(
m_pidL.calculate(m_encoderL.getRate(), speeds.leftMetersPerSecond)
- + m_feedforward.calculate(speeds.leftMetersPerSecond)
- );
-
- m_motorR_leader.setVoltage(
+ + m_feedforward.calculate(speeds.leftMetersPerSecond),
m_pidR.calculate(m_encoderR.getRate(), speeds.rightMetersPerSecond)
- + m_feedforward.calculate(speeds.rightMetersPerSecond)
+ + m_feedforward.calculate(speeds.rightMetersPerSecond)
);
}
+ public void voltage(double leftVolts, double rightVolts)
+ {
+ m_motorL_leader.setVoltage(leftVolts);
+ m_motorR_leader.setVoltage(rightVolts);
+ }
+
public Pose2d pose2d()
{
- System.out.println("Distance: "+m_encoderL.getDistance());
- System.out.println("Distance: "+m_encoderR.getDistance());
+ System.out.println("Distance: " + m_encoderL.getDistance());
+ System.out.println("Distance: " + m_encoderR.getDistance());
return m_odometry.getPoseMeters();
}
@@ -95,6 +105,7 @@ public class Drivetrain extends SubsystemBase
m_gyro.reset();
m_encoderL.reset();
m_encoderR.reset();
+
m_odometry = new DifferentialDriveOdometry(
m_gyro.toRotation2d(),
Constants.encoderL.getDistance(),
@@ -116,4 +127,10 @@ public class Drivetrain extends SubsystemBase
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
}
+
+ public Command trajectory(Trajectory trajectory)
+ {
+ // return this.runOnce(...)
+ return Commands.none();
+ }
} \ No newline at end of file
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
index 3860b85..8d41c63 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -39,15 +39,15 @@ public class IMUGyro
mpu = gyro;
reset();
- // mpu.write(CONFIG_ADDR, 0b000000);
- mpu.write(SMPLRT_DIV, 0b00000010);
- // mpu.write(GYRO_CONFIG, 0b00011000);
- // mpu.write(ACCL_CONFIG, 0b00000000);
+ mpu.write(CONFIG_ADDR, 0b00000100);
+ mpu.write(SMPLRT_DIV, 0b00010011);
+ mpu.write(GYRO_CONFIG, 0b00011000);
+ mpu.write(ACCL_CONFIG, 0b00000000);
}
public void reset()
{
- mpu.write(PWR_MGMT_1, 0b000001000);
+ mpu.write(PWR_MGMT_1, 0b00000000);
pitch = 0;
roll = 0;
generated by cgit v1.2.3 (git 2.25.1) at 2025-10-31 06:44:18 +0000