Working Turret Version

Coconuts2486-FRC · Feb 19, 2020 · 8ea26c2 · 8ea26c2
1 parent 9047721
commit 8ea26c2
Show file tree

Hide file tree

Showing 13 changed files with 49 additions and 56 deletions.
diff --git a/FRC2020/.gradle/6.0.1/executionHistory/executionHistory.bin b/FRC2020/.gradle/6.0.1/executionHistory/executionHistory.bin
diff --git a/FRC2020/.gradle/6.0.1/executionHistory/executionHistory.lock b/FRC2020/.gradle/6.0.1/executionHistory/executionHistory.lock
diff --git a/FRC2020/.gradle/6.0.1/fileHashes/fileHashes.bin b/FRC2020/.gradle/6.0.1/fileHashes/fileHashes.bin
diff --git a/FRC2020/.gradle/6.0.1/fileHashes/fileHashes.lock b/FRC2020/.gradle/6.0.1/fileHashes/fileHashes.lock
diff --git a/FRC2020/.gradle/6.0.1/javaCompile/classAnalysis.bin b/FRC2020/.gradle/6.0.1/javaCompile/classAnalysis.bin
diff --git a/FRC2020/.gradle/6.0.1/javaCompile/javaCompile.lock b/FRC2020/.gradle/6.0.1/javaCompile/javaCompile.lock
diff --git a/FRC2020/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/FRC2020/.gradle/buildOutputCleanup/buildOutputCleanup.lock
diff --git a/FRC2020/src/main/java/frc/robot/Map.java b/FRC2020/src/main/java/frc/robot/Map.java
@@ -54,6 +54,7 @@ public static class controllers{
             public static int manuelLauncherAddPower = 4;
             public static int manuelLauncherSubtractPower = 2;
             public static int manuelMode = 10; // button that changes remote into manuel
+            public static int manuelEncoderZeroer = 7;
         }
     }
         public static class Cartridge{

diff --git a/FRC2020/src/main/java/frc/robot/Robot.java b/FRC2020/src/main/java/frc/robot/Robot.java
@@ -1,20 +1,19 @@
 package frc.robot;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+
 import edu.wpi.first.wpilibj.TimedRobot;
-import frc.robot.Cartridge.pistonlift;
-import frc.robot.TeleOp.DriveTrain;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Turret.TurretControl;
 import frc.robot.Turret.TurretDisplay;
 import frc.robot.Turret.TurretMotion;
-import frc.robot.Vision.LimeLight;
 import frc.robot.Vision.Pixy;
 
 public class Robot extends TimedRobot {
   @Override
   public void robotInit() {
-    TurretMotion.Rotation.setPosition(0); // Resets encoder ticks
+    TurretMotion.init();
     Pixy.init(); // Starts up the Pixy2 Camera
-    //Map.Turret.motors.rotation.configSelectedFeedbackSensor(FeedbackDevice.None, 0, 10);
   }
 
   public void robotPeriodic(){
@@ -23,6 +22,7 @@ public void robotPeriodic(){
 
   @Override
   public void autonomousInit() {
+    Map.Turret.motors.rotation.setSelectedSensorPosition(0);
   }
 
   @Override
@@ -31,6 +31,7 @@ public void autonomousPeriodic() {
 
   @Override
   public void teleopInit() {
+    Map.Turret.motors.rotation.setNeutralMode(NeutralMode.Coast);
   }
 
   @Override
@@ -39,8 +40,8 @@ public void teleopPeriodic() {
     TurretDisplay.display(); // Displays LimeLight stats
     //pixyControl.run(); // Runs pixy homing automation
 
-    DriveTrain.drive();
-    pistonlift.run();
+    //DriveTrain.drive();
+    //pistonlift.run();
   }
 
   @Override
@@ -49,6 +50,10 @@ public void testInit() {
 
   @Override
   public void testPeriodic() {
+    if(Map.Controllers.driverLeft.getRawButtonPressed(3)){
+      Map.Turret.motors.rotation.setSelectedSensorPosition(0);
+    }
+    SmartDashboard.putNumber("Position!!! ", TurretMotion.Rotation.getDegrees());
   }
 
 }
diff --git a/FRC2020/src/main/java/frc/robot/Turret/Targeting.java b/FRC2020/src/main/java/frc/robot/Turret/Targeting.java
@@ -1,9 +1,7 @@
 package frc.robot.Turret;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
-
 import frc.robot.Map;
-import frc.robot.Cartridge.Conveyor;
 import frc.robot.Vision.LimeLight;
 
 /**
@@ -32,7 +30,6 @@ public class Targeting {
 
     public static void initilize() {
         // Starts tracking process
-        LimeLight.LED.on();
         // Sets launching motors at base speed
         Thread setBaseLaunchingSpeed = new Thread() {
             public void run() {
@@ -72,26 +69,24 @@ public static void stop() {
         launcherUpToSpeed = false;
         maintainBaseLaunchSpeed = false;
         LimeLight.LED.off();
+        TurretMotion.Rotation.turn(0);
     }
 
     private static void setBaseLaunchingSpeed() {
-        while (maintainBaseLaunchSpeed) {
+        while (maintainBaseLaunchSpeed&&track) {
             TurretMotion.Launcher.setVelocity(baseLaunchSpeed);
         }
     }
 
     private static void findTarget() {
         // Finds target
         if (!LimeLight.isTarget()) {
-            while (!LimeLight.isTarget()) {
-                if (TurretMotion.Rotation.getDegrees() >= 90) {
-                    while (TurretMotion.Rotation.getDegrees() < 180 || (!LimeLight.isTarget())) {
-                        TurretMotion.Rotation.turn(1);
-                    }
-                } else {
-                    while (TurretMotion.Rotation.getDegrees() > 0 || (!LimeLight.isTarget())) {
-                        TurretMotion.Rotation.turn(-1);
-                    }
+            while (!LimeLight.isTarget()&&track) {
+                while(!LimeLight.isTarget()&&TurretMotion.Rotation.getDegrees()<180&&track){
+                    TurretMotion.Rotation.turn(0.5);
+                }
+                while(!LimeLight.isTarget()&&TurretMotion.Rotation.getDegrees()>0&&track){
+                    TurretMotion.Rotation.turn(-0.5);
                 }
             }
         }
@@ -116,7 +111,7 @@ private static void initLauncher() {
         double targetSpeed = calculateLaunchSpeed();
         double abserror = Math.abs(targetSpeed - motorSpeed);
         double error = targetSpeed - motorSpeed;
-        while (maintainLaunchingSpeed) {
+        while (maintainLaunchingSpeed&&track) {
             motorSpeed = TurretMotion.Launcher.getVelocity();
             targetSpeed = calculateLaunchSpeed();
             abserror = Math.abs(targetSpeed - motorSpeed);
@@ -163,7 +158,7 @@ private static void trackTarget() {
                 targetZeroedIn = true;
                 TurretMotion.Rotation.turn(0);
             }else{
-                //findTarget();
+                findTarget();
             }
         }
     }

diff --git a/FRC2020/src/main/java/frc/robot/Turret/TurretControl.java b/FRC2020/src/main/java/frc/robot/Turret/TurretControl.java
@@ -22,6 +22,7 @@ public static void run() {
         if(Map.Controllers.xbox.getRawButtonPressed(Map.Turret.controllers.manuelMode)){
             if(manuelMode){
                 manuelMode = false;
+                led = false;
                 LimeLight.LED.off();
             }else{
                 manuelMode = true;
@@ -47,10 +48,13 @@ public static void run() {
             double large = Math.abs(Map.Controllers.xbox.getRawAxis(Map.Turret.controllers.manuelRotateLarge));
             double small = Math.abs(Map.Controllers.xbox.getRawAxis(Map.Turret.controllers.manuelRotateSmall));
             if(large>small){
-                TurretMotion.Rotation.turn(-Map.Controllers.xbox.getRawAxis(Map.Turret.controllers.manuelRotateLarge)/manuelLargeAdjusterDivision);
+                TurretMotion.Rotation.turn(Map.Controllers.xbox.getRawAxis(Map.Turret.controllers.manuelRotateLarge)/manuelLargeAdjusterDivision);
             }else{
-                TurretMotion.Rotation.turn(-Map.Controllers.xbox.getRawAxis(Map.Turret.controllers.manuelRotateSmall)/manuelSmallAdjusterDivision);
+                TurretMotion.Rotation.turn(Map.Controllers.xbox.getRawAxis(Map.Turret.controllers.manuelRotateSmall)/manuelSmallAdjusterDivision);
             }
+            if(Map.Controllers.xbox.getRawButtonPressed(Map.Turret.controllers.manuelEncoderZeroer)){
+                Map.Turret.motors.rotation.setSelectedSensorPosition(0);
+              }
             if(Map.Controllers.xbox.getRawButtonPressed(1)){
                 if(!led){
                   LimeLight.LED.on();

diff --git a/FRC2020/src/main/java/frc/robot/Turret/TurretDisplay.java b/FRC2020/src/main/java/frc/robot/Turret/TurretDisplay.java
@@ -11,6 +11,7 @@ public static void display(){
         SmartDashboard.putNumber("LimeLight X: ", LimeLight.getX());
         SmartDashboard.putNumber("LimeLight Y: ", LimeLight.getY());
         SmartDashboard.putBoolean("LimeLight Target:", LimeLight.isTarget());
-        SmartDashboard.putNumber("Lime Val", LimeLight.table.getEntry("tv").getDouble(5.5));
+        SmartDashboard.putNumber("Lime Val", LimeLight.table.getEntry("tv").getDouble(-5));
+        SmartDashboard.putNumber("Position!!! ", TurretMotion.Rotation.getDegrees());
     }
 }
diff --git a/FRC2020/src/main/java/frc/robot/Turret/TurretMotion.java b/FRC2020/src/main/java/frc/robot/Turret/TurretMotion.java
@@ -1,18 +1,22 @@
 package frc.robot.Turret;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.SensorCollection;
-
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import frc.robot.Map;
 
 /**
  * TurretMotion
  */
 public class TurretMotion {
+    public static void init(){
+        Map.Turret.motors.follower.follow(Map.Turret.motors.launcher);
+        Map.Turret.motors.rotation.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10);
+        Map.Turret.motors.rotation.setNeutralMode(NeutralMode.Coast);
+        Map.Turret.motors.rotation.setInverted(true);
+        TurretMotion.Rotation.setPosition(0);
+    }
     public static class Launcher{
-        public static void init(){
-            Map.Turret.motors.follower.follow(Map.Turret.motors.launcher);
-        }
         public static void setPercentSpeed(double speed){
             Map.Turret.motors.launcher.set(ControlMode.PercentOutput, speed);
         }
@@ -25,22 +29,9 @@ public static double getVelocity(){
     }
     public static class Rotation{
 
-        private static int ticksInrevolution = 5000; // needs to be changed to actual number
-        private static SensorCollection sensors = new Map.Turret.motors().rotation.getSensorCollection();
-        private volatile int lastValue = Integer.MIN_VALUE;
-        public int getPwmPosition() {
-            int raw = sensors.getPulseWidthRiseToFallUs();
-            if (raw == 0) {
-            int lastValue = this.lastValue;
-            if (lastValue == Integer.MIN_VALUE) {
-            return 0;
-            }
-            return lastValue;
-            }
-            int actualValue = Math.min(4096, raw - 128);
-            lastValue = actualValue;
-            return actualValue;
-            }
+        private static int ticksInrevolution = 4095; // needs to be changed to actual number
+        private static int maxDeg = 180;
+        private static int minDeg = 0;
 
         public static void setPosition(int pos){
             // Used to reset the known position of the turret
@@ -63,19 +54,15 @@ public static double getDegrees(){
         }
         public static void turn(double pwr){
             // Turns turret using pwr as input (-1 to 1)
-            Map.Turret.motors.rotation.set(ControlMode.PercentOutput, pwr);
-            /*
-            if(pwr>0){
-                if(getDegrees()<180){
+            //Map.Turret.motors.rotation.set(ControlMode.PercentOutput, pwr);
+                if(pwr>0&&getDegrees()<=maxDeg){
                     Map.Turret.motors.rotation.set(ControlMode.PercentOutput, pwr);
-                }
-            }else if(pwr<0){
-                if(getDegrees()>0){
+                }else if(pwr<0&&getDegrees()>minDeg){
                     Map.Turret.motors.rotation.set(ControlMode.PercentOutput, pwr);
+                }else{
+                    Map.Turret.motors.rotation.set(ControlMode.PercentOutput, 0);
                 }
-                
-            }
-            */
+
         }
     }
 }