working gyro and subsystem

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;