Merge pull request #7 from Team4388/gyro_fix

Gyro fix

src/main/java/frc4388/robot/Robot.java

@@ -7,9 +7,17 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.commands.AutoBalanceTF2;
+import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
 
 /**
@@ -25,6 +33,35 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
+  public static class MicroBot extends SubsystemBase {
+    public WPI_Pigeon2 pigeon = new WPI_Pigeon2(14);
+    public RobotGyro gyro = new RobotGyro(pigeon);
+
+    public TalonSRX frontLeft = new TalonSRX(2);
+    public TalonSRX backLeft = new TalonSRX(3);
+    public TalonSRX backRight = new TalonSRX(5);
+    public TalonSRX frontRight = new TalonSRX(4);
+
+    public MicroBot() {
+      frontRight.configFactoryDefault();
+      frontLeft.configFactoryDefault();
+      backLeft.configFactoryDefault();
+      backRight.configFactoryDefault();
+  
+      frontLeft.setInverted(true);
+      backLeft.setInverted(true);
+    }
+
+    public void setOutput(double output) {
+      frontRight.set(ControlMode.PercentOutput, output);
+      frontLeft.set(ControlMode.PercentOutput, output);
+      backLeft.set(ControlMode.PercentOutput, output);
+      backRight.set(ControlMode.PercentOutput, output);
+    }
+  }
+
+  private MicroBot bot = new MicroBot();
+
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
@@ -34,6 +71,8 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
+    bot.setDefaultCommand(new AutoBalanceTF2(bot));
   }
 
   /**
@@ -106,6 +145,8 @@ public class Robot extends TimedRobot {
       m_autonomousCommand.cancel();
     }
     m_robotTime.startMatchTime();
+
+    m_robotContainer.gyroRef.reset();
   }
 
   /**
@@ -113,7 +154,9 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    
+    SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
+    SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
+    SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -16,6 +16,7 @@ import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.RobotGyro;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
 
@@ -39,6 +40,8 @@ public class RobotContainer {
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
 
+    public RobotGyro gyroRef = m_robotMap.gyro;
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */

src/main/java/frc4388/robot/RobotMap.java

@@ -9,6 +9,7 @@ package frc4388.robot;
 
 import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.PigeonIMU;
@@ -27,6 +28,10 @@ import frc4388.utility.RobotGyro;
  * testing and modularization.
  */
 public class RobotMap {
+    private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
+    public RobotGyro gyro = new RobotGyro(m_pigeon2);
+
+    private TalonSRX backleft = new TalonSRX(-1);
 
   public SwerveModule leftFront;
   public SwerveModule rightFront;

src/main/java/frc4388/robot/commands/AutoBalanceTF2.java

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.math.MathUtil;
+import frc4388.robot.Robot;
+
+// NOTE:	Consider using this command inline, rather than writing a subclass.	For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class AutoBalanceTF2 extends PelvicInflamitoryDisease {
+	Robot.MicroBot bot;
+
+	/** Creates a new AutoBalanceTF2. */
+	public AutoBalanceTF2(Robot.MicroBot bot) {
+		super(.7, .1, 15, 0);
+		addRequirements(bot);
+		this.bot = bot;
+	}
+
+	@Override
+	public double getError() {
+		return bot.gyro.getPitch();
+	}
+
+	@Override
+	public void runWithOutput(double output) {
+		double out2 = MathUtil.clamp(output / 40, -.5, .5);
+		if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0;
+		bot.setOutput(out2);
+	}
+
+	@Override
+	public void initialize() {
+		super.initialize();
+		this.bot.gyro.reset();
+	}
+
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}

src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.utility.Gains;
+
+public abstract class PelvicInflamitoryDisease extends CommandBase {
+	protected Gains gains;
+	private double output = 0;
+
+	/** Creates a new PelvicInflamitoryDisease. */
+	public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) {
+		gains = new Gains(kp, ki, kd, kf, 0);
+	}
+
+	public PelvicInflamitoryDisease(Gains gains) {
+		this.gains = gains;
+	}
+
+	/** produces the error from the setpoint */
+	public abstract double getError();
+	/** figure it out bitch */
+	public abstract void runWithOutput(double output);
+
+	// Called when the command is initially scheduled.
+	@Override
+	public void initialize() {
+		output = 0;
+	}
+
+	private double prevError, cumError = 0;
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override
+	public void execute() {
+		double error = getError();
+		cumError += error * .02; // 20 ms
+		double delta = error - prevError;
+
+		output = error * gains.kP;
+		output += cumError * gains.kI;
+		output += delta * gains.kD;
+		output += gains.kF;
+
+		runWithOutput(output);
+	}
+
+	// Called once the command ends or is interrupted.
+	@Override
+	public void end(boolean interrupted) {}
+
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}

src/main/java/frc4388/utility/RobotGyro.java

@@ -7,7 +7,9 @@
 
 package frc4388.utility;
 
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
 import com.kauailabs.navx.frc.AHRS;
 
@@ -21,18 +23,21 @@ import edu.wpi.first.math.MathUtil;
 public class RobotGyro implements Gyro {
     private RobotTime m_robotTime = RobotTime.getInstance();
 
-    private PigeonIMU m_pigeon = null;
+    private WPI_Pigeon2 m_pigeon = null;
     private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
     private double m_lastPigeonAngle;
     private double m_deltaPigeonAngle;
 
+    private double pitchZero = 0;
+    private double rollZero = 0;
+
     /**
      * Creates a Gyro based on a pigeon
      * @param gyro the gyroscope to use for Gyro
      */
-    public RobotGyro(PigeonIMU gyro) {
+    public RobotGyro(WPI_Pigeon2 gyro) {
         m_pigeon = gyro;
         m_isGyroAPigeon = true;
     }
@@ -46,6 +51,16 @@ public class RobotGyro implements Gyro {
         m_isGyroAPigeon = false;
     }
 
+    /**
+     * Resets yaw, pitch, and roll.
+     */
+    public void resetZeroValues() {
+        if (!m_isGyroAPigeon) return;
+
+        pitchZero = m_pigeon.getPitch();
+        rollZero =  m_pigeon.getRoll();
+    }
+
     /**
      * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
      * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
@@ -75,7 +90,7 @@ public class RobotGyro implements Gyro {
     @Override
     public void calibrate() {
         if (m_isGyroAPigeon) {
-            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+            m_pigeon.calibrate();
         } else {
             m_navX.calibrate();
         }
@@ -83,6 +98,8 @@ public class RobotGyro implements Gyro {
 
     @Override
     public void reset() {
+        resetZeroValues();
+
         if (m_isGyroAPigeon) {
             m_pigeon.setYaw(0);
         } else {
@@ -99,9 +116,10 @@ public class RobotGyro implements Gyro {
 	 *					Roll is within [-90,+90] degrees.
      */
     private double[] getPigeonAngles() {
-        double[] angles = new double[3];
-        m_pigeon.getYawPitchRoll(angles);
-        return angles;
+        double[] ypr = new double[3];
+        m_pigeon.getYawPitchRoll(ypr);
+
+        return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
     }
 
     @Override
@@ -113,6 +131,10 @@ public class RobotGyro implements Gyro {
         }
     }
 
+    public double getYaw() {
+       return this.getAngle();
+    }
+
     /**
      * Gets an absolute heading of the robot
      * @return heading from -180 to 180 degrees
@@ -166,7 +188,7 @@ public class RobotGyro implements Gyro {
         }
     }
 
-    public PigeonIMU getPigeon(){
+    public WPI_Pigeon2 getPigeon(){
         return m_pigeon;
     }