Merge pull request #15 from Coconuts2486-FRC/main

Fully Functioning Teleop XD

FRC2020/src/main/java/frc/robot/Autonomous/AutoCommands.java

@@ -203,34 +203,6 @@ public static void strafeDiagnol(double strafeSetpoint, String direction){
 
         }
 
-    }
-
-    public static void driveStraight(double degreeSetpoint){
-        double pl = 0;
-        double pr = 0;
-
-        Map.driveTrain.gyro.getYawPitchRoll(PID.turnPID.ypr_deg);
-
-        double error = PID.turnPID.ypr_deg[0]/90;
-
-        Map.driveTrain.lf.set(-.3 - error);
-        Map.driveTrain.lr.set(-.3 - error);
-        Map.driveTrain.rf.set(-.3 + error);
-        Map.driveTrain.rr.set(-.3 + error);
-
-        /*
-        while (PID.turnPID.ypr_deg[0] < degreeSetpoint ){
-            pl -=.1;
-        }
-
-        while (PID.turnPID.ypr_deg[0] > degreeSetpoint){
-            pr -=.1;
-        }
-        */
-
-
-
-
     }
 
     public static void stop(){

FRC2020/src/main/java/frc/robot/Autonomous/AutoMissions.java

@@ -1,6 +1,8 @@
 package frc.robot.Autonomous;
 
 import frc.robot.Map;
+import frc.robot.Autonomous.Commands.AutoCommands3;
+import frc.robot.Utilities.Sleep;
 
 public class AutoMissions {
 
@@ -21,6 +23,49 @@ public static void AutoInit(){
         Map.driveTrain.gyro.setYaw(0);
 
     }
+    public static void test(){
+        if(!AutoCommands3.ran){
+            AutoCommands3.turnToAngle(90,1);
+            Sleep.delay(1000);
+            AutoCommands3.turnToAngle(0,-1);
+            /*
+            AutoCommands3.endAuto();
+            AutoCommands3.turnToAngle(90, 1);
+            */
+            AutoCommands3.endAuto();
+        }
+    }
+    public static void trenchRun2(){
+        if(!AutoCommands3.ran){
+            //Launch first set and get ready for second set
+            AutoCommands3.Turret.goTo(80.73);
+            AutoCommands3.Turret.init();
+            AutoCommands3.Turret.launch(3);
+            AutoCommands3.goDistance(8.3,1); // goes up to first ball
+            AutoCommands3.wait(AutoCommands3.Turret.hasLaunched, true);
+            AutoCommands3.Piston.on();
+            //Launches all three balls, one at a time
+            AutoCommands3.Loading.load();
+            AutoCommands3.goDistance(3,0.2);
+            AutoCommands3.Loading.stop();
+            AutoCommands3.Turret.launch(1);
+            AutoCommands3.wait(AutoCommands3.Turret.hasLaunched, true);
+
+            AutoCommands3.Loading.load();
+            AutoCommands3.goDistance(3,0.2);
+            AutoCommands3.Loading.stop();
+            AutoCommands3.Turret.launch(1);
+            AutoCommands3.wait(AutoCommands3.Turret.hasLaunched, true);
+
+            AutoCommands3.Loading.load();
+            AutoCommands3.goDistance(3,0.2);
+            AutoCommands3.Loading.stop();
+            AutoCommands3.Turret.launch(1);
+            AutoCommands3.wait(AutoCommands3.Turret.hasLaunched, true);
+            // Ends auto
+            AutoCommands3.endAuto();
+          }
+    }
 
     public static void NoAutoSelected() {
         CurrentAuto = "None Selected";

FRC2020/src/main/java/frc/robot/Autonomous/Commands/AutoCommands3.java

@@ -1,8 +1,10 @@
 package frc.robot.Autonomous.Commands;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
+
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Map;
+import frc.robot.Autonomous.PID;
 import frc.robot.Cartridge.FullSensor;
 import frc.robot.Turret.Targeting;
 import frc.robot.Turret.TurretMotion;
@@ -14,52 +16,82 @@
  */
 public class AutoCommands3 {
     public static boolean ran = false;
+    public static double range = 0.0;
     public static void endAuto(){
         ran = true;
         Turret.stop();
         stop();
-        Map.Cartridge.RightPiston.set(false);
+        Map.Cartridge.piston.set(false);
         Loading.load();
         Loading.load = false;
-        AutoCommands3.Turret.goTo(0);
+        //AutoCommands3.Turret.goTo(90);
     }
     public static void wait(boolean value, boolean toBe){
         while(!(value=toBe)){
 
         }
     }
-    public static void goDistance(double feet,double speed){
+    public static void goDistance(double feet,double speedModifier){//works, but be careful about slight drift, lower power won't matter. when it starts, it drifts a bit, but starightens out. Solved through aligning center of balls with roughly the middle of the front right wheel
         Distance.setEncoders(0);
+        Map.driveTrain.gyro.setYaw(0);
         Sleep.delay(100);
         feet = Math.abs(feet);
+        double turnError = PID.turnPID.ypr_deg[0]/90;
         double distanceTravelled = Distance.distanceTravelled();
+        double driveError = feet - distanceTravelled;
+        double speed = PID.drivePID.kP * driveError;
         while(distanceTravelled<feet&&(!ran)){
+            SmartDashboard.putNumber("turnError", turnError);
+            SmartDashboard.putNumber("gyro angle", PID.turnPID.ypr_deg[0]);
+            SmartDashboard.putNumber("distance", distanceTravelled);
+            Map.driveTrain.gyro.getYawPitchRoll(PID.turnPID.ypr_deg);
+            turnError = PID.turnPID.ypr_deg[0]/90;
             distanceTravelled = Distance.distanceTravelled();
-            Distance.drive(speed);
+            driveError = feet - distanceTravelled;
+            speed = PID.drivePID.kP * driveError;
+            Distance.drive(speed * speedModifier, turnError);
         }
         stop();
     }
-    public static void turnToAngle(double angle){
-        angle = 0-angle;
-        double[] ypr = new double[3];
-        Map.driveTrain.gyro.getYawPitchRoll(ypr);
-        double currentAngle = ypr[0];
-        while(!Gyro.isAtAngle(currentAngle,angle)&&(!ran)){
-            Map.driveTrain.gyro.getYawPitchRoll(ypr);
-            currentAngle = ypr[0];
-            SmartDashboard.putNumber("Angle", currentAngle);
-            if(Math.abs(currentAngle-angle)>Gyro.slopePoint){
-                if(currentAngle>angle){
-                   turn(-Gyro.maxSpeed);
-                }else{
-                    turn(Gyro.maxSpeed);
+    public static void turnToAngle(double angle, int direction){//could make a turn left and turn right function, this should keep it to one
+        //angle = 0-angle;
+        Map.driveTrain.gyro.setYaw(0);
+        Sleep.delay(100);
+        Map.driveTrain.gyro.getYawPitchRoll(PID.turnPID.ypr_deg);
+        double currentAngle = PID.turnPID.ypr_deg[0];
+        double spinError = angle - currentAngle;
+        double speed = PID.turnPID.kP * spinError;
+        if(direction == 1){
+            int errorCount = 100;
+            int currentErrorCount = 0;
+            while (currentErrorCount<errorCount && (!ran)){
+                Map.driveTrain.gyro.getYawPitchRoll(PID.turnPID.ypr_deg);// LOGAN!!! READ!!!! THIS!!!!once currentAngle > angle, pid can't correct back. Need to tell it to not exit until its at set angle for a couple seconds
+                currentAngle = PID.turnPID.ypr_deg[0];
+                spinError = angle - currentAngle;
+                speed = PID.turnPID.kP * spinError;
+                if(Math.abs(spinError)<1){
+                    currentErrorCount++;
                 }
-            }else{
-                turn(-((currentAngle-angle)/(Gyro.slopePoint+angle)));
+                turn(speed);
+            }
+        }else if(direction == -1){
+            int errorCount = 100;
+            int currentErrorCount = 0;
+            while (currentErrorCount<errorCount && (!ran)){
+                Map.driveTrain.gyro.getYawPitchRoll(PID.turnPID.ypr_deg);
+                currentAngle = PID.turnPID.ypr_deg[0];
+                spinError = angle - currentAngle;
+                speed = PID.turnPID.kP * spinError;
+                if(Math.abs(spinError)<1){
+                    currentErrorCount++;
+                }
+                turn(speed);
             }
         }
         stop();
     }
+
+
     private static void turn(double pwr){
         Map.driveTrain.lf.set(pwr);
         Map.driveTrain.rf.set(-pwr);
@@ -117,9 +149,9 @@ public static void on(){
                 public void run(){
                     piston = true;
                     while(piston&&(!ran)){
-                        Map.Cartridge.RightPiston.set(true);
+                        Map.Cartridge.piston.set(true);
                     }
-                    Map.Cartridge.RightPiston.set(false);
+                    Map.Cartridge.piston.set(false);
                 }
             };
             thread.start();
@@ -160,11 +192,13 @@ public static void stop(){
         }
     }
     private static class Distance{
-        private static void drive(double pwr){
-            Map.driveTrain.lf.set(-pwr);
-            Map.driveTrain.rf.set(-pwr);
-            Map.driveTrain.lr.set(-pwr);
-            Map.driveTrain.rr.set(-pwr);
+        private static void drive(double pwr, double turnError){
+
+
+            Map.driveTrain.lf.set(-pwr - turnError);
+            Map.driveTrain.rf.set(-pwr + turnError);
+            Map.driveTrain.lr.set(-pwr - turnError);
+            Map.driveTrain.rr.set(-pwr + turnError);
         }
         private static double ticksToFeet(double ticks){
             double ticksToFeet = (6 * Math.PI) / 84;
@@ -183,10 +217,11 @@ private static void setEncoders(double set){
 
         }
     }
+    /*
     private static class Gyro{
-        private static double slopePoint = 90; //degrees
-        private static double maxSpeed = 1; // 0-1.00 percent
-        private static double error = 3; //angle error
+        private static double slopePoint = 18; //degrees
+        private static double maxSpeed = 0.5; // 0-1.00 percent
+        private static double error = 0.5; //angle error
         private static boolean isAtAngle(double currentAngle, double targetAngle){
             double absError = Math.abs(currentAngle-targetAngle);
             if(absError>error){
@@ -196,6 +231,7 @@ private static boolean isAtAngle(double currentAngle, double targetAngle){
             }
         }
     }
+    */
     private static void stop(){
         Map.driveTrain.lf.set(0);
         Map.driveTrain.rf.set(0);

FRC2020/src/main/java/frc/robot/Autonomous/PID.java

@@ -23,7 +23,7 @@ public static class drivePID{
 
     public static class turnPID{
 
-        final static double kP = 0.05;
+        public final static double kP = 0.1;
         final static double kI = 0;
         final static double kD = 0;
         final static double iLimit = 10;

FRC2020/src/main/java/frc/robot/Cartridge/Conveyor.java

@@ -17,16 +17,16 @@ public static void run() {
         if (Map.Controllers.driverRight.getRawButton(Map.Cartridge.controllers.intake)) {
             if (FullSensor.getSensorValue()&&(!TurretSettings.turretUsingConveyors)) {
                 Map.Cartridge.ArmRoller.set(ControlMode.PercentOutput, 1); // adjust speeds
-                Map.Cartridge.Conveyor1.set(ControlMode.PercentOutput, 0.3); // adjust speeds
-                Map.Cartridge.Conveyor2.set(ControlMode.PercentOutput, 0.1);
-                Map.Cartridge.Conveyor3.set(ControlMode.PercentOutput, 0.1);
+                Map.Cartridge.Conveyor1.set(ControlMode.PercentOutput, 0.5); // adjust speeds
+                Map.Cartridge.Conveyor2.set(ControlMode.PercentOutput, 0.3);
+                Map.Cartridge.Conveyor3.set(ControlMode.PercentOutput, 0.3);
             } else if(!TurretSettings.turretUsingConveyors){
                 Map.Cartridge.ArmRoller.set(ControlMode.PercentOutput, 1); // adjust speeds
                 Map.Cartridge.Conveyor1.set(ControlMode.PercentOutput, 0);
                 Map.Cartridge.Conveyor2.set(ControlMode.PercentOutput, 0);
                 Map.Cartridge.Conveyor3.set(ControlMode.PercentOutput, 0);
             }
-        }else if((Map.Controllers.driverLeft.getRawButton(Map.Cartridge.controllers.outtake)||Map.Controllers.driverRight.getRawButton(Map.Cartridge.controllers.outtake))&&(!TurretSettings.turretUsingConveyors)){
+        }else if((Map.Controllers.driverRight.getRawButton(Map.Cartridge.controllers.outtake))&&(!TurretSettings.turretUsingConveyors)){
             outtake();
         }else if(!TurretSettings.turretUsingConveyors){
             stop();

FRC2020/src/main/java/frc/robot/Cartridge/pistonlift.java

@@ -9,10 +9,10 @@ public class pistonlift {
     public static void run() {
         if (Map.Controllers.driverLeft.getRawButtonPressed(1)) {
             if (!pistonactive) {
-                Map.Cartridge.RightPiston.set(true);
+                Map.Cartridge.piston.set(true);
                 pistonactive = true;
             } else {
-                Map.Cartridge.RightPiston.set(false);
+                Map.Cartridge.piston.set(false);
                 Map.Cartridge.ArmRoller.set(ControlMode.PercentOutput, 0);
                 Map.Cartridge.Conveyor1.set(ControlMode.PercentOutput, 0);
                 pistonactive = false;

FRC2020/src/main/java/frc/robot/Climber/Climb.java

@@ -1,7 +1,73 @@
 package frc.robot.Climber;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Map;
+
 public class Climb{
-//uses a sparkMax
+
+    public static void Init(){
+
+        Map.climber.leftClimb.setNeutralMode(NeutralMode.Brake);
+        Map.climber.rightClimb.setNeutralMode(NeutralMode.Brake);
+
+        Map.climber.leftClimb.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10);
+        Map.climber.rightClimb.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10);
+
+        Map.climber.rightClimb.setInverted(true);
+        Map.climber.leftClimb.setInverted(false);
+        Map.climber.leftClimb.setSensorPhase(true);
+
+        Map.climber.leftClimb.getSensorCollection().setQuadraturePosition(0, 10);
+        Map.climber.rightClimb.getSensorCollection().setQuadraturePosition(0, 10);
+    }
+    public static void display(){
+        SmartDashboard.putNumber("RightClimber Position: ", Map.climber.rightClimb.getSelectedSensorPosition());
+        SmartDashboard.putNumber("LeftClimber Position: ", Map.climber.leftClimb.getSelectedSensorPosition());
+        SmartDashboard.putNumber("LEFT Velocity", Map.climber.leftClimb.getSelectedSensorVelocity());
+        SmartDashboard.putNumber("RIGHT Velocity", Map.climber.rightClimb.getSelectedSensorVelocity());
+    }
+    private static boolean climberActivated = false;
+    public static void run(){
+            if (Map.Controllers.driverRight.getRawButton(Map.climber.lift)&&climberActivated){
+                lift(0.5);
+            }
+            if(Map.Controllers.driverRight.getRawButtonPressed(Map.climber.unlock)){
+                if(!climberActivated){
+                    climberActivated=true;
+                }else{
+                    climberActivated=false;
+                }
+            }
+            if(climberActivated){
+                Map.climber.lock.set(false);
+            }else{
+                Map.climber.lock.set(true);
+            }
+    }
+    private static double maxError = 500;
+    private static double maxHight = 80000;
+    private static void lift(double pwr){
+        double leftP = Map.climber.leftClimb.getSelectedSensorPosition();
+        double rightP = Map.climber.rightClimb.getSelectedSensorPosition();
+        double error = leftP-rightP;
+        double absError = Math.abs(error);
+        if(absError>maxError){
+            absError = maxError;
+        }
+        double speedSetting = 1-(absError/maxError);
+        double averageHight = (leftP+rightP)/2;
+            if(averageHight<maxHight){
+                Map.climber.leftClimb.set(ControlMode.PercentOutput, ((speedSetting-(error/maxError))));
+                Map.climber.rightClimb.set(ControlMode.PercentOutput,((speedSetting+(error/maxError))));
+            }else{
+                Map.climber.leftClimb.set(ControlMode.PercentOutput, 0);
+                Map.climber.rightClimb.set(ControlMode.PercentOutput,0);
+            }
+    }
+
 
 
 

FRC2020/src/main/java/frc/robot/Color_Wheel/Arm.java

@@ -115,11 +115,8 @@ public static void colorMatchRed(){
 
     }
 
-    public static void spin(){
-        if(Map.Controllers.xbox.getRawButton(50)){
-            Map.ColorWheel.ColorSpinner.set(ControlMode.PercentOutput, .5);
-        }
-
+    public static void spin(double pwr){
+            Map.ColorWheel.ColorSpinner.set(ControlMode.PercentOutput, pwr);
     }
 
 }