Grabber on early tuesday

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2019.2.1"
+    id "edu.wpi.first.GradleRIO" version "2019.4.1"
 }
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"

src/main/java/frc/robot/DriveTrain.java

@@ -22,7 +22,7 @@ public class DriveTrain {
     private final int rightPort1;
     private final int leftPort2;
     private final int rightPort2;
-    public final ADIS16448_IMU gyro = new ADIS16448_IMU();
+    //public final ADIS16448_IMU gyro = new ADIS16448_IMU();
 
     private int counter = 0;
 
@@ -42,7 +42,7 @@ public DriveTrain(final int leftPort1, final int leftPort2, final int rightPort1
             rightMotor2.setNeutralMode(NeutralMode.Brake);
             leftMotor1.setNeutralMode(NeutralMode.Brake);
             leftMotor2.setNeutralMode(NeutralMode.Brake);
-            gyro.reset();
+      //      gyro.reset();
             //gyroPortNumber should be analong 0 or 1
 
     }
@@ -80,48 +80,22 @@ public void updateSpeed(final ThrottlePosition throttlePosition) {
         rightMotor1.set(ControlMode.PercentOutput, right);
         rightMotor2.follow(rightMotor1);
 
-        counter++;
-        if(counter % 10 == 0){
-            double left1Current = leftMotor1.getOutputCurrent();
-            double left1Voltage = leftMotor1.getMotorOutputVoltage();
-            double right1Current = rightMotor1.getOutputCurrent();
-            double right1Voltage = rightMotor1.getMotorOutputVoltage();
-            double left2Voltage = leftMotor2.getMotorOutputVoltage();
-            double right2Voltage = rightMotor2.getMotorOutputVoltage();
-            System.out.print(String.format("Currents: R: %.2f L: %.2f ", right1Current, left1Current));
-            System.out.print(String.format("Voltage: L1: %.2f R1: %.2f L2: %.2f R2: %.2f\n", left1Voltage, right1Voltage, left2Voltage, right2Voltage));
-
-        }
-        //makeVictorsFollowers();
     }
     public void autoUpdateSpeed(double left, double right) {
         leftMotor1.set(ControlMode.PercentOutput, left);
         rightMotor1.set(ControlMode.PercentOutput, right);
         leftMotor2.follow(leftMotor1);
         rightMotor2.follow(rightMotor1);
-
-        counter++;
-        if(counter % 10 == 0){
-           double left1Current = leftMotor1.getOutputCurrent();
-           double left1Voltage = leftMotor1.getMotorOutputVoltage();
-           double right1Current = rightMotor1.getOutputCurrent();
-           double right1Voltage = rightMotor1.getMotorOutputVoltage();
-           double left2Voltage = leftMotor2.getMotorOutputVoltage();
-           double right2Voltage = rightMotor2.getMotorOutputVoltage();
-           System.out.print(String.format("Currents: R: %.2f L: %.2f ", right1Current, left1Current));
-           System.out.print(String.format("Voltage: L1: %.2f R1: %.2f L2: %.2f R2: %.2f\n", left1Voltage, right1Voltage, left2Voltage, right2Voltage));
-
-        }
     }
     public TalonSRX getLeftMotor() {
         return leftMotor1;
     }
     public TalonSRX getRightMotor() {
         return rightMotor1;
     }
-    public ADIS16448_IMU getGyro() {
-        return gyro;
-    }
+    //public ADIS16448_IMU getGyro() {
+      //  return gyro;
+    //}
 
     public void getEncoderPosition(){
         int encoderPositionLeft = leftMotor1.getSelectedSensorPosition();

src/main/java/frc/robot/Elevator.java

@@ -36,6 +36,8 @@ public Elevator(int elevatorPort , int limitSwitchPort){
         talon = new TalonSRX(elevatorPort);
         talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
         talon.setSensorPhase(true);
+        talon.configMotionCruiseVelocity(1100);
+        talon.configMotionAcceleration(1100);
         talon.setSelectedSensorPosition(0);
         // talon.config_kD(0, dConstant);
         // talon.config_kP(0, pConstant);
@@ -58,9 +60,9 @@ public void configurePID() {
     //@param distance is in inches
     public void move(double distance){
         talon.setIntegralAccumulator(0);
-        int talonPosition =  talon.getSelectedSensorPosition();
+        //int talonPosition =  talon.getSelectedSensorPosition();
         double totalPulses = (distance/(diameter*Math.PI)) * pulsesPerRevolution;
-        talon.set(ControlMode.Position, totalPulses);
+        talon.set(ControlMode.MotionMagic, totalPulses);
         double pulsesAfter = talon.getSelectedSensorPosition();
     }
 

src/main/java/frc/robot/Grabber.java

@@ -9,10 +9,11 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Grabber {
+    //this worked
     private VictorSP leftFlywheel;
     private VictorSP rightFlywheel;
     private TalonSRX rightClaw;
-    private TalonSRX leftClaw;
+    public TalonSRX leftClaw;
     private DigitalInput leftLimitSwitch;
     private DigitalInput rightLimitSwitch;
     private final double flywheelSpeed = 0.5;
@@ -25,6 +26,7 @@ public class Grabber {
     private final double openRevolutions = 12.5;
     private final double cargoRevolutions = 8.33;
     private final double hatchRevolutions = 1.389;
+    private final double testRevolution = 1.389*2;
     private final double closedRevolutions = 0;
 
 
@@ -40,28 +42,42 @@ public Grabber(int leftFlywheelPort , int rightFlywheelPort , int leftClawPort ,
         SmartDashboard.putNumber("pid/claw/d", 0.0);
         SmartDashboard.putNumber("pid/claw/f", 0.0);
 
-        //rightClaw.config_kD(0, dConstant);
-        //rightClaw.config_kP(0, pConstant);
-        //rightClaw.config_kI(0, iConstant);
-       // rightClaw.config_kF(0, fConstant);
         rightClaw.configPeakCurrentLimit(maxAmps);
         rightClaw.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
+        rightClaw.setInverted(true);
+        rightClaw.setSensorPhase(true);
 
-        //leftClaw.config_kD(0, dConstant);
-        //leftClaw.config_kP(0, pConstant);
-        //leftClaw.config_kI(0, iConstant);
-        //leftClaw.config_kF(0, fConstant);
         leftClaw.configPeakCurrentLimit(maxAmps);
         leftClaw.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
+        leftClaw.setSensorPhase(true);
 
+        leftClaw.configMotionCruiseVelocity(3000);
+        leftClaw.configMotionAcceleration(6000);
+
+        rightClaw.configMotionCruiseVelocity(3000);
+        rightClaw.configMotionAcceleration(6000);
+
+
+        rightClaw.setSelectedSensorPosition(0);
+        leftClaw.setSelectedSensorPosition(0);
         leftLimitSwitch = new DigitalInput(leftLimitPort);
         rightLimitSwitch = new DigitalInput(rightLimitPort);
     }
+
+    public void initializeSettings(){
+        rightClaw.configPeakCurrentLimit(maxAmps);
+        rightClaw.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
+        leftClaw.setSensorPhase(true);
+        leftClaw.configPeakCurrentLimit(maxAmps);
+        leftClaw.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
+
+    }
     public void configurePID() {
         this.pConstant = SmartDashboard.getNumber("pid/claw/p", 0.2);
         this.iConstant = SmartDashboard.getNumber("pid/claw/i", pConstant / 10000);
         this.dConstant = SmartDashboard.getNumber("pid/claw/d", 0.0);
         this.fConstant = SmartDashboard.getNumber("pid/claw/f", 0.0);
+        this.iConstant = (this.iConstant*this.pConstant)/1000;
         leftClaw.config_kD(0, dConstant);
         leftClaw.config_kP(0, pConstant);
         leftClaw.config_kI(0, iConstant);
@@ -71,25 +87,72 @@ public void configurePID() {
         rightClaw.config_kP(0, pConstant);
         rightClaw.config_kI(0, iConstant);
         rightClaw.config_kF(0, fConstant);
+
     }
     public void open(){
-        move(openRevolutions);
+        try {
+            move(openRevolutions);
+        } catch (InterruptedException e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+        }
 
     }  
 
     public void cargo(){
-        move(cargoRevolutions);
+        try {
+            move(cargoRevolutions);
+        } catch (InterruptedException e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+        }
     }  
     public void hatch(){
-        move(hatchRevolutions);
+        try {
+            move(hatchRevolutions);
+        } catch (InterruptedException e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+        }
     }
     public void closed(){
-        move(closedRevolutions);
+        try {
+            move(closedRevolutions);
+        } catch (InterruptedException e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+        }
     }
-    public void move(double numRevolutions){
-            double totalPulses = numRevolutions * pulsesPerRevolution;
-            leftClaw.set(ControlMode.Position, totalPulses);
-            rightClaw.set(ControlMode.Position , totalPulses);
+    public void move(double numRevolutions) throws InterruptedException {
+        leftClaw.setIntegralAccumulator(0);
+        rightClaw.setIntegralAccumulator(0);
+        leftClaw.setSelectedSensorPosition(0);
+        rightClaw.setSelectedSensorPosition(0);
+        double totalPulses = numRevolutions * pulsesPerRevolution;
+        System.out.println("About to move");
+        //leftClaw.set(ControlMode.MotionMagic, totalPulses);
+        rightClaw.set(ControlMode.MotionMagic , totalPulses);
+        System.out.println("Move complete");
+
+        double motorVoltage = leftClaw.getMotorOutputVoltage();
+
+       // double pulsesAfter = rightClaw.getSelectedSensorPosition();
+        //int closedLoopError = rightClaw.getClosedLoopError();
+        //double motorOutput = rightClaw.getMotorOutputPercent();
+        double leftPulsesAfter = leftClaw.getSelectedSensorPosition();
+        double leftClosedLoopError = leftClaw.getClosedLoopError();
+        double leftMotorVoltage = leftClaw.getMotorOutputVoltage();
+       // System.out.println("Right Voltage: " + motorVoltage + "Position after: " + pulsesAfter + "Closed Loop Error" + closedLoopError);
+        System.out.println("Left Voltage: " + leftMotorVoltage + "Position after: " + leftPulsesAfter + "Closed Loop Error" + leftClosedLoopError);
+    }
+
+    public void testTenDegrees(){
+        try {
+            move(testRevolution);
+        } catch (InterruptedException e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+        }
     }
 
     public void toLimitSwitches(){
@@ -110,25 +173,41 @@ public void toLimitSwitches(){
 
     }
 
-    public void intake(){
+    public void output(){
         leftFlywheel.set(flywheelSpeed);
         rightFlywheel.set(-flywheelSpeed);
 
     }
 
-    public void output(){
+    public void intake(){
         leftFlywheel.set(-flywheelSpeed);
         rightFlywheel.set(flywheelSpeed);
     }
 
+    public void zeroPosition(){
+        leftClaw.setSelectedSensorPosition(0);
+        rightClaw.setSelectedSensorPosition(0);
+    }
+
     public void stop(){
         leftFlywheel.set(0);
         rightFlywheel.set(0);
     }
 
-    public void resetToFactorySettings(){
-        rightClaw.configFactoryDefault();
-        leftClaw.configFactoryDefault();
+    public void testEncoderPosition(){
+        double rightBeginningPosition = rightClaw.getSelectedSensorPosition();
+        double leftBeginningPosition = leftClaw.getSelectedSensorPosition();
+        System.out.println("Left: " + leftBeginningPosition + " Right: " + rightBeginningPosition);
+        //leftClaw.set(ControlMode.PercentOutput , 0.2);
+        double leftAfter = leftClaw.getSelectedSensorPosition();
+        double rightAfter = rightClaw.getSelectedSensorPosition();
+        System.out.println("Left: " + leftAfter + " Right: " + rightAfter);
+
+    }
+
+    public void spinClawMotor(){
+        leftClaw.set(ControlMode.PercentOutput , 0.4);
+        //rightClaw.set(ControlMode.PercentOutput , 0.4);
     }
 
 

src/main/java/frc/robot/MotionProfiling.java

@@ -45,13 +45,13 @@ public MotionProfiling(DriveTrain driveTrain, String setupLeft , String setupRig
     public void update() { 
         double l = left.calculate(leftMotor.getSelectedSensorPosition());
         double r = right.calculate(rightMotor.getSelectedSensorPosition());
-        double gyroHeading = driveTrain.getGyro().getAngleY();   // Assuming the gyro is giving a value in degrees
-        double desiredHeading = -Pathfinder.r2d(left.getHeading());  // Should also be in degrees
+        //double gyroHeading = driveTrain.getGyro().getAngleY();   // Assuming the gyro is giving a value in degrees
+        //double desiredHeading = -Pathfinder.r2d(left.getHeading());  // Should also be in degrees
 
-        double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading);
-        double turn = 0.8 * (-1.0/80.0) * angleDifference;
+        //double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading);
+        //double turn = 0.8 * (-1.0/80.0) * angleDifference;
 
-        driveTrain.autoUpdateSpeed(l + turn, r - turn);
+        //driveTrain.autoUpdateSpeed(l + turn, r - turn);
     }
 
     public boolean isFinished() {

src/main/java/frc/robot/Robot.java

@@ -51,16 +51,20 @@ public void robotInit() {
     elevator.configurePID();
     grabber.configurePID();
     arm.configurePID();
-    elevator.resetToFactorySettings();
-    grabber.resetToFactorySettings();
+    //elevator.resetToFactorySettings();
+    //grabber.resetToFactorySettings();
     driveTrain.autoUpdateSpeed(0,0);
     for (int i = 0; i < pathFiles.length; i++) {
       pathFiles[i] = String.format(filePath , i + 1);
     }
-    joystick1.addButton(2, elevator::lowCargo);
-    joystick1.addButton(3, elevator::lowHatch);
-    joystick1.addButton(4, elevator::mediumHigh);
-    joystick1.addButton(5, elevator::zeroLimitSwitch);
+    joystick1.addButton(2, grabber::hatch);
+    joystick1.addButton(3, grabber::cargo);
+    joystick1.addButton(4, grabber::open);
+    //joystick1.addButton(8 , grabber::intake , grabber::stop);
+    //joystick1.addButton(9 , grabber::output , grabber:: stop);
+    joystick1.addButton(8 , elevator::lowCargo);
+    joystick1.addButton(9 , elevator::lowHatch);
+
   }
 
   /**
@@ -117,9 +121,13 @@ public void teleopInit(){
     elevator.configurePID();
     grabber.configurePID();
     arm.configurePID();
-    elevator.resetToFactorySettings();
-    grabber.resetToFactorySettings();
+    grabber.zeroPosition();
+    //elevator.resetToFactorySettings();
+    //grabber.resetToFactorySettings();
+
+    //grabber.initializeSettings();
     driveTrain.autoUpdateSpeed(0,0);
+
   }
 
 
@@ -156,6 +164,9 @@ public void teleopPeriodic() {
     else{
       joystick1.listen();  
     }
+    System.out.println("Voltage: " + grabber.leftClaw.getMotorOutputVoltage() + "Error: " + grabber.leftClaw.getClosedLoopError() + "Current:" + grabber.leftClaw.getOutputCurrent());
+
+
   }
 
 public void changeDriverControl(){  
@@ -174,5 +185,7 @@ private void updateSmartDB(){
    */
   @Override
   public void testPeriodic() {
+    grabber.testEncoderPosition();
+
   }
 }