Theses changes were my attempt at fixing the errors in the code Charl…

…es pushed a little earlier today (6/1/2021). Signed-off-by: Alex Pereira <[email protected]>
2199robolions · Jun 1, 2021 · a8bcdae · a8bcdae
1 parent cd55706
commit a8bcdae
Show file tree

Hide file tree

Showing 7 changed files with 136 additions and 104 deletions.
diff --git a/.vscode/launch.json b/.vscode/launch.json
@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "CodeLens (Launch) - Main",
+      "request": "launch",
+      "mainClass": "frc.robot.Main",
+      "projectName": "2021SwerveRobot"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }
diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java
@@ -5,10 +5,7 @@
 import edu.wpi.first.wpiutil.math.MathUtil;
 
 public class Controls {
-    private boolean button3State = false;
-    private boolean oldButton3State = false;
-    private boolean fieldDrive = false;
-
+
     private enum ControllerIDs {
         JOYSTICK(1),
         XBOXCONTROLLER(0);
@@ -89,7 +86,7 @@ public double getRotatePower() {
      * Gets the drive power
      * @return drivePower
      */
-    public double getDrivePower(){
+    public double getDrivePower() {
         double x = joystick.getX();
         double y = joystick.getY() * -1;
 
@@ -106,7 +103,7 @@ public double getDrivePower(){
      * Gets the drive X
      * @return driveX
      */
-    public double getDriveX(){
+    public double getDriveX() {
         double power = joystick.getX();
         double deadZone = 0.1;
 
@@ -122,7 +119,7 @@ public double getDriveX(){
      * Gets the drive Y
      * @return driveY
      */
-    public double getDriveY(){
+    public double getDriveY() {
         double power = joystick.getY() * -1;
         double deadZone = 0.1;
 
@@ -134,14 +131,14 @@ public double getDriveY(){
         }        
     }
 
-    public boolean fieldDrive(){
-        oldState = currState;
-        currState = joystick.getRawButton(3);
+    /**
+     * 
+     * @return Whether or not field oriented drive should be activated
+     */
+    public boolean getFieldDrive() {
+        boolean fieldDrive;
+        fieldDrive = joystick.getRawButton(3);
 
-        //If the button was just pressed
-        if((currState == true) && (oldState == false)) {
-            fieldDrive != fieldDrive; //Switch the fieldDrive value
-        }
         return fieldDrive;
     }
 

diff --git a/src/main/java/frc/robot/Drive.java b/src/main/java/frc/robot/Drive.java
@@ -2,12 +2,12 @@
 
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import frc.robot.enums.RobotStatus;
-import com.kauailabs.navx.frc.AHRS;
 
 public class Drive {
-
+    
     // An enum containing each wheel's properties including: drive and rotate motor IDs, drive motor types, and rotate sensor IDs 
     public enum WheelProperties {
         // TODO: All of the below 0's should be replaced with real ID numbers
@@ -39,7 +39,6 @@ public enum WheelProperties {
         private double targetRadians;
         private double targetVoltage;
         private int offsetDegrees; //Inverse of the reading when wheel is physically at 0 degrees
-        private AHRS ahrs;
 
         // Each item in the enum will now have to be instantiated with a constructor with the all of the ids and the motor type constants. Look few lines above, where FRONT_RIGHT_WHEEL(int driveMotorId, MotorType driveMotorType, int rotateMotorId, int rotateSensorId, double targetRadians, double targetVoltage), REAR_LEFT_WHEEL(int driveMotorId, MotorType driveMotorType, int rotateMotorId, int rotateSensorId, double targetRadians, double targetVoltage), etc... are. These are what the constructor is for.
         private WheelProperties(int driveMotorId, int rotateMotorId, int rotateSensorId, double targetRadians, int offsetDegrees) {
@@ -138,28 +137,11 @@ public double getAngle()  {
         }
     }
 
-
+    /**
+     * Contructor for the Drive class
+     */
     public Drive() {
-        try {
-			ahrs = new AHRS(SPI.Port.kMXP);
-		} catch (RuntimeException ex) {
-			System.out.println("Error Instantiating navX MXP: " + ex.getMessage());
-		}
-
-		ahrs.reset();
-
-		while (ahrs.isConnected() == false) {
-			// System.out.println("Connecting navX");
-		}
-		System.out.println("navX Connected");
-
-		while (ahrs.isCalibrating() == true) {
-			System.out.println("Calibrating navX");
-		}
-		System.out.println("navx Ready");
-
-		// At Start, Set navX to ZERO
-		ahrs.zeroYaw();
+        //
     }
 
     public PowerAndAngle calcSwerve(double driveX, double driveY, double rotatePower, double rotateAngle){
@@ -246,10 +228,6 @@ public void teleopRotate(double rotatePower) {
         rearRightWheel.rotateAndDrive(rotateRightRearMotorAngle, rotatePower * -1);
         rearLeftWheel.rotateAndDrive(rotateLeftRearMotorAngle, rotatePower * -1);
     }
-
-    public double getYaw(){
-        return ahrs.getYaw();
-    }
 
     public void teleopRotateOld(double rotatePower) {
         /**

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -110,7 +110,6 @@ public void teleopPeriodic() {
     //Sets the color of the LED's (when we get them)
     led.defaultMode(m_colorSelected);
     //drive.teleopRotate(rotatePower);
-
   }
 
   @Override
@@ -149,11 +148,4 @@ public void crabDrive() {
     drive.teleopCrabDrive(wheelAngle, drivePower);
   }
 
-  public static void getFieldDrive(){
-    return controls.fieldDrive();
-  }
-  public static void getYaw(){
-    return drive.getYaw();
-  }
-
 } // End of the Robot Class
diff --git a/src/main/java/frc/robot/Wheel.java b/src/main/java/frc/robot/Wheel.java
@@ -3,13 +3,27 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.VictorSP;
+
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
-import edu.wpi.first.wpilibj.VictorSP;
+import edu.wpi.first.wpilibj.SPI;
+
+import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.wpilibj.controller.PIDController;
+
 import edu.wpi.first.wpiutil.math.MathUtil;
 
 public class Wheel {
+    // Variables
+    private boolean currbutton3State = false;
+    private boolean oldButton3State  = false;
+    private boolean fieldDrive       = false;
+
+    //private double currWheelAngle;
+
+    //Object Creation
+    Controls controls;
 
     // Motor Controllers Declaration (instantiated in the constructor in order to dependency inject the IDs of each respective controller)
     private CANSparkMax driveMotor;
@@ -25,8 +39,8 @@ public class Wheel {
     private AnalogPotentiometer rotateMotorSensor;
     private PIDController rotationPID;
 
-    //PID Controller Declaration
-    //private PIDController pidController = new PIDController(kP, kI, kD);
+    //NAVX
+    private AHRS ahrs;
 
     // PID Controller Values (static, as these constants will not change for each individual motor)
     // TODO: make sure to replace the 0.0's with actual values
@@ -46,9 +60,34 @@ public Wheel(int driveMotorID, int rotateMotorID, int rotateMotorSensorID, int o
         //Sensor measures from above going Counter clockwise
         rotateMotorSensor = new AnalogPotentiometer(rotateMotorSensorID, -360, offsetDegrees);
 
+        //NAVX
+		try {
+			ahrs = new AHRS(SPI.Port.kMXP);
+		} catch (RuntimeException ex) {
+			System.out.println("Error Instantiating navX MXP: " + ex.getMessage());
+		}
+
+		ahrs.reset();
+
+		while (ahrs.isConnected() == false) {
+			// System.out.println("Connecting navX");
+		}
+		System.out.println("navX Connected");
+
+		while (ahrs.isCalibrating() == true) {
+			System.out.println("Calibrating navX");
+		}
+		System.out.println("navx Ready");
+
+		// At Start, Set navX to ZERO
+        ahrs.zeroYaw();
+
         //PID Controller
         rotationPID = new PIDController(kP, kI, kD);
         rotationPID.enableContinuousInput(-180, 180);
+
+        //Instance Creation
+        controls = new Controls();
     }
 
     /**
@@ -70,14 +109,14 @@ public void rotateAndDrive(double targetWheelAngle, double drivePower) {
         // 10       .1
         // 5        .05
 
-        if (Robot.getFieldDrive() == false) {
-            currWheelAngle = getRotateMotorPosition();
-        }
-        else if (Robot.getFieldDrive() == true){
+        if (fieldDrive() == true){
             //In field drive, the wheels' angles will be the robot's direction + the wheels' directions
-            currWheelAngle = adjustValue(getRotateMotorPosition() + Robot.getYaw());
+            currWheelAngle = adjustValue(getRotateMotorPosition() + getYaw());
+        }
+        else {
+            //This occurs whenever Field Drive is not enabled, and for any other strange cases
+            currWheelAngle = getRotateMotorPosition();
         }
-
 
         /**
          * If PID is 0 to 360
@@ -182,4 +221,24 @@ else if(adjustedValue < -180) {
         return adjustedValue;
     }
 
+    /**
+     * The getyYaw function for the NavX
+     * @return The NavX's Yaw
+     */
+    public double getYaw(){
+        return ahrs.getYaw();
+    }
+
+    public boolean fieldDrive() {
+        oldButton3State  = currbutton3State;
+        currbutton3State = controls.getFieldDrive();
+
+        //If the button was just pressed
+        if((currbutton3State == true) && (oldButton3State == false)) {
+            fieldDrive =! fieldDrive; //Switch the fieldDrive value
+        }
+
+        return fieldDrive;
+    }
+
 } // End of the Wheel Class