Merge pull request #46 from Team4388/fix-drive-train-pids

Fix drive train pids

src/main/java/frc4388/robot/Constants.java

@@ -40,7 +40,7 @@ public final class Constants {
         public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor
         public static final double NEUTRAL_DEADBAND = 0.04;
         public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
-            new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01);
+            new SupplyCurrentLimitConfiguration(false, 60, 40, 2);
         public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops
 
         /* Drive Train Characteristics */
@@ -55,8 +55,8 @@ public final class Constants {
         public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
         public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55);
         public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
-        public static final int DRIVE_CRUISE_VELOCITY = 20000;
-        public static final int DRIVE_ACCELERATION = 7000;
+        public static final int DRIVE_CRUISE_VELOCITY = 25000;
+        public static final int DRIVE_ACCELERATION = 21000;
 
         public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
         public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
@@ -104,8 +104,8 @@ public final class Constants {
     }
     
     public static final class IntakeConstants {
-        public static final int INTAKE_SPARK_ID = -9;
-        public static final int EXTENDER_SPARK_ID = -10;
+        public static final int INTAKE_SPARK_ID = 12;
+        public static final int EXTENDER_SPARK_ID = 13;
     }
   
     public static final class ShooterConstants {

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

@@ -30,7 +30,10 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.commands.DrivePositionMPAux;
 import frc4388.robot.commands.DriveStraightAtVelocityPID;
+import frc4388.robot.commands.DriveStraightToPositionMM;
+import frc4388.robot.commands.DriveStraightToPositionPID;
 import frc4388.robot.commands.DriveWithJoystick;
+import frc4388.robot.commands.GotoCoordinates;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
 import frc4388.robot.commands.RunIntakeWithTriggers;
@@ -114,28 +117,28 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new TurnDegrees(90, m_robotDrive));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
+            .whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
      
         /* Driver Buttons */
         // sets solenoids into high gear
         new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
 
         // sets solenoids into low gear
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
 
         /* Operator Buttons */
         //TODO: Shooter Buttons
@@ -189,28 +192,28 @@ public class RobotContainer {
        
         //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
         //This assumes that we are positioned against the right wall with our shooter facing the target.
-        return new SequentialCommandGroup(new Wait(2, m_robotDrive), 
+        return new SequentialCommandGroup(new Wait(0, m_robotDrive), 
                                     //add aim command
                                     //add shooter command
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0),
-                                    new ParallelCommandGroup(
+                                    //new DriveStraightToPositionMM(m_robotDrive, 48.0),
+                                    /*new ParallelCommandGroup(
                                         new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
+                                            new DriveStraightToPositionMM(m_robotDrive, 36.0)),
                                     new ParallelCommandGroup(
                                          new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
-                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                            new DriveStraightToPositionMM(m_robotDrive, 36.0)),
+                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),*/
                                     //add aim command 
                                     //add shooter command
 //Below this would be the picking up additional balls outside of those in the trench directly behind us
 
-                                    new TurnDegrees(-150, m_robotDrive),
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0),
-                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                    new TurnDegrees(75, m_robotDrive),
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0),
-                                    new TurnDegrees(-45, m_robotDrive),
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0));
+                                    //new GotoCoordinates(m_robotDrive, 36, 36),
+                                    new GotoCoordinates(m_robotDrive, 36, 36, -90));//,
+                                    //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                    //new TurnDegrees(m_robotDrive, 75),
+                                    //new DriveStraightToPositionMM(m_robotDrive, 18.0),
+                                    //new TurnDegrees(m_robotDrive, -45),
+                                    //new DriveStraightToPositionMM(m_robotDrive, 12.0));
     }
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(

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

@@ -66,13 +66,13 @@ public class DrivePositionMPAux extends CommandBase {
     if (System.currentTimeMillis() - m_startTime < m_rampRate) {
       // Ramping
       m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
-      m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+      m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
     } else if (m_targetPos - m_currentPos > m_rampDist) {
       // Cruising
-      m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
+      m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro);
     } else {
       // Deramp PID
-      m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
+      m_drive.runDrivePositionPID(m_targetPos, m_targetGyro);
     }
     m_counter ++;
   }

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

@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
   @Override
   public void execute() {
     //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
-    m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+    m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -28,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase {
   public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }

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

@@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
@@ -58,7 +58,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){
+    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){
       return true;
     } else {
       i++;

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

@@ -39,27 +39,43 @@ public class DriveWithJoystick extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double moveInput = m_controller.getRightXAxis();
-    double steerInput = -m_controller.getLeftYAxis();
+    double moveInput = -m_controller.getLeftYAxis();
+    double steerInput = m_controller.getRightXAxis();
     double moveOutput = 0;
     double steerOutput = 0;
-    if (steerInput >= 0){
-      steerOutput = -Math.cos(1.571*steerInput)+1;
+    if (moveInput >= 0){
+      moveOutput = -Math.cos(1.571*moveInput)+1;
     } else {
-      steerOutput = Math.cos(1.571*steerInput)-1;
+      moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
     double cosMultiplier = 1.0;
     double deadzone = .1;
-    if (moveInput > 0){
-      moveOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*moveInput) + cosMultiplier;
-    } else if (moveInput < 0) {
-      moveOutput = (cosMultiplier - deadzone) * Math.cos(1.571*moveInput) - cosMultiplier;
+    if (steerInput > 0){
+      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
+    } else if (steerInput < 0) {
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
     } else {
-      moveOutput = 0;
+      steerOutput = 0;
+    }
+    double tempOutputLimit = 0.8;
+
+    boolean isOutputLimited = false;
+
+    if (isOutputLimited) {
+      if (moveOutput > tempOutputLimit) {
+        moveOutput = tempOutputLimit;
+      } else if(moveOutput < -tempOutputLimit) {
+        moveOutput = -tempOutputLimit;
+      }
+
+      if (steerOutput > tempOutputLimit) {
+        steerOutput = tempOutputLimit;
+      } else if(steerOutput < -tempOutputLimit) {
+        steerOutput = -tempOutputLimit;
+      }
     }
     
-    SmartDashboard.putNumber("Steer Output Test", moveOutput);
     m_drive.driveWithInput(moveOutput, steerOutput);
   }
 

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

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class GotoCoordinates extends SequentialCommandGroup {
+  Drive m_drive;
+
+  double m_xTarget;
+  double m_yTarget;
+  double m_currentAngle;
+  double m_hypotDist;
+  double m_endAngle;
+
+  /**
+   * Creates a new GotoPosition.
+   */
+  public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+
+    m_xTarget = xTarget;
+    m_yTarget = yTarget;
+
+    m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
+
+    m_currentAngle = calcAngle();
+
+    SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle); 
+
+    m_endAngle = endAngle;
+
+
+    addCommands(  new TurnDegrees(m_drive, m_currentAngle), 
+                  new DriveStraightToPositionMM(m_drive, m_hypotDist), 
+                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
+  }
+
+  public boolean isQuadrantThree() {
+    if ((m_xTarget < 0) && (m_yTarget < 0)) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  public double calcAngle() {
+    if (isQuadrantThree()) {
+      return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
+    } else {
+      return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
+    }
+  }
+
+}

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

@@ -41,15 +41,15 @@ public class RunIntakeWithTriggers extends CommandBase {
     double rightTrigger = m_controller.getRightTriggerAxis();
     double leftTrigger = m_controller.getLeftTriggerAxis();
     double output = 0;
-    if (rightTrigger < .5) {
-      if(rightTrigger > leftTrigger) {
-        output = rightTrigger;
+    if (leftTrigger < .5) {
+      if(leftTrigger > rightTrigger) {
+        output = leftTrigger;
       }
-      if (leftTrigger > rightTrigger) {
+      if (rightTrigger > leftTrigger) {
         output = -leftTrigger;
       }
     } else {
-      output = rightTrigger;
+      output = leftTrigger;
     }
     m_intake.runIntake(output);
   }

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

@@ -24,7 +24,7 @@ public class TurnDegrees extends CommandBase {
   /**
    * Creates a new TurnDeg.
    */
-  public TurnDegrees(double targetAngle, Drive subsystem) {
+  public TurnDegrees(Drive subsystem, double targetAngle) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_targetAngle = targetAngle;

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -88,6 +88,7 @@ public class Drive extends SubsystemBase {
   public double m_lastAngleYaw = 0;
   public double m_currentAngleYaw = 0;
 
+  public double m_lastAngleGotoCoordinates;
   /* Smart Dashboard Objects */
   SendableChooser<String> m_songChooser = new SendableChooser<String>();
   
@@ -322,7 +323,7 @@ public class Drive extends SubsystemBase {
    * using the Differential Drive class to manage the two inputs
    */
   public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(move, steer);
+    m_driveTrain.arcadeDrive(steer, move);
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
   }
@@ -726,6 +727,12 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
       //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
 
+      double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+
+      SmartDashboard.putNumber("Left Motor RPM", leftRPM);
+      SmartDashboard.putNumber("Right Motor RPM", rightRPM);
+
       //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
       //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
       //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());