Cube Manipulator / Robot Map Tweaks

FIRST1939 · Feb 14, 2018 · acc0413 · acc0413
1 parent 31e3f7e
commit acc0413
Show file tree

Hide file tree

Showing 10 changed files with 70 additions and 24 deletions.
diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/Robot.java b/src/main/java/com/frcteam1939/powerup2018/robot/Robot.java
@@ -26,6 +26,7 @@
 import com.frcteam1939.powerup2018.util.AutonomousOptions;
 
 import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.command.Command;
@@ -101,6 +102,8 @@ public void robotInit() {
  SmartDashboard.putData(Scheduler.getInstance());
  SmartDashboard.putData(new FindMaxSpeed());
 
+ CameraServer.getInstance().startAutomaticCapture();
+
  System.out.println(" Finished Intializing");
  System.out.println("==========================================/n");
  }

diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/RobotMap.java b/src/main/java/com/frcteam1939/powerup2018/robot/RobotMap.java
@@ -15,14 +15,12 @@ public class RobotMap {
  public static final int slaveCubeManipulatorTalon = 13;
 
  // Solenoids
-
- // One double solenoid on 0 and 1, second double solenoid on 6 and 7, one single solenoid on 2
  public static final int PCM = 0;
  public static final int leftShiftingGearboxUp = 0;
- public static final int leftShiftingGearboxDown = 0;
- public static final int rightShiftingGearboxUp = 0;
- public static final int rightShiftingGearboxDown = 0;
- public static final int cubeManipulatorAngleTop = 0;
+ public static final int leftShiftingGearboxDown = 1;
+ public static final int rightShiftingGearboxUp = 6;
+ public static final int rightShiftingGearboxDown = 7;
+ public static final int cubeManipulatorAngleTop = 5;
  public static final int cubeManipulatorAngleBottom = 3;
  public static final int cubeManipulatorWheelsSolenoid = 2;
 

diff --git a/...frcteam1939/powerup2018/robot/commands/cubemanipulator/CubeManipulatorGamepadControl.java b/...frcteam1939/powerup2018/robot/commands/cubemanipulator/CubeManipulatorGamepadControl.java
@@ -35,7 +35,7 @@ protected void execute() {
  Robot.cubeManipulator.set(CubeManipulator.OUT_SPEED);
  if (!Robot.cubeManipulator.haveCube()) {
  Robot.cubeManipulator.set(0);
- Robot.cubeManipulator.cubeManipulatorRaise();
+ Robot.cubeManipulator.cubeManipulatorMiddle();
  }
  }
 
@@ -45,7 +45,7 @@ protected void execute() {
  Robot.cubeManipulator.set(CubeManipulator.OUT_SPEED);
  if (!Robot.cubeManipulator.haveCube()) {
  Robot.cubeManipulator.set(0);
- Robot.cubeManipulator.cubeManipulatorRaise();
+ Robot.cubeManipulator.cubeManipulatorMiddle();
  }
  }
  }

diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/commands/cubemanipulator/OutputCube.java b/src/main/java/com/frcteam1939/powerup2018/robot/commands/cubemanipulator/OutputCube.java
@@ -10,6 +10,6 @@ public OutputCube() {
  this.addSequential(new CubeManipulatorLower());
  this.addSequential(new SetCubeManipulatorSpeed(CubeManipulator.OUT_SPEED));
  this.addSequential(new CubeManipulatorWheelsOut());
- this.addSequential(new CubeManipulatorRaise());
+ this.addSequential(new CubeManipulatorMiddle());
  }
 }
diff --git a/...java/com/frcteam1939/powerup2018/robot/commands/smartdashboard/SmartDashboardUpdater.java b/...java/com/frcteam1939/powerup2018/robot/commands/smartdashboard/SmartDashboardUpdater.java
@@ -31,6 +31,9 @@ protected void execute() {
  SmartDashboard.putNumber("Heading", Robot.drivetrain.getHeading());
 
  SmartDashboard.putNumber("Elevator Revolutions", Robot.elevator.getRevolutions());
+ SmartDashboard.putNumber("Elevator Height", Robot.elevator.getHeight());
+ SmartDashboard.putNumber("Elevator Speed", Robot.elevator.getSpeed());
+
  SmartDashboard.putNumber("Climber Revolutions", Robot.climber.getPosition());
 
  SmartDashboard.putNumber("Pressure", Robot.getPressure());

diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/Climber.java b/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/Climber.java
@@ -14,11 +14,20 @@ public class Climber extends Subsystem {
 
  TalonSRX talon = new TalonSRX(RobotMap.climberTalon);
 
+ private static final int TIMEOUT_MS = 0;
+
  private static final int CPR = 4096;
  public double MAX_REV = 0;
 
  public Climber() {
- this.talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
+ this.talon.setNeutralMode(NeutralMode.Brake);
+ this.talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, TIMEOUT_MS);
+ this.talon.configOpenloopRamp(2, TIMEOUT_MS);
+ this.talon.configNominalOutputForward(+0, TIMEOUT_MS);
+ this.talon.configNominalOutputReverse(-0, TIMEOUT_MS);
+ this.talon.configPeakOutputForward(+1, TIMEOUT_MS);
+ this.talon.configPeakOutputReverse(-1, TIMEOUT_MS);
+ this.talon.enableVoltageCompensation(true);
  }
 
  @Override

diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/CubeManipulator.java b/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/CubeManipulator.java
@@ -1,6 +1,7 @@
 package com.frcteam1939.powerup2018.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.frcteam1939.powerup2018.robot.Robot;
 import com.frcteam1939.powerup2018.robot.RobotMap;
@@ -27,6 +28,8 @@ public class CubeManipulator extends Subsystem {
  public CubeManipulator() {
  // slaveTalon.setInverted(true);
  this.slaveTalon.follow(this.masterTalon);
+ this.masterTalon.setNeutralMode(NeutralMode.Coast);
+ this.slaveTalon.setNeutralMode(NeutralMode.Coast);
  }
 
  @Override

diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/Drivetrain.java b/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/Drivetrain.java
@@ -26,9 +26,9 @@
 
 public class Drivetrain extends Subsystem {
 
- private static final int TIMEOUT_MS = 10;
+ private static final int TIMEOUT_MS = 0;
 
- private static final double lowGearLimit = 1.0;
+ private static final double lowGearLimit = .5;
  private static final int MAX_SPEED_LOW = 0;
  private static final int MAX_SPEED_HIGH = 0;
 
@@ -94,7 +94,7 @@ public double getRightSpeed() {
  }
 
  public double getLeftPosition() {
- return this.frontLeft.getSelectedSensorPosition(0) / CPR * WHEEL_CIRCUMFERENCE; // Divide by CPR, multiply by circumference, any additional calc
+ return this.frontLeft.getSelectedSensorPosition(0) / CPR * WHEEL_CIRCUMFERENCE;
  }
 
  public double getRightPosition() {

diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/Elevator.java b/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/Elevator.java
@@ -3,6 +3,7 @@
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.frcteam1939.powerup2018.robot.DistanceConstants;
 import com.frcteam1939.powerup2018.robot.RobotMap;
@@ -12,18 +13,19 @@
 
 public class Elevator extends Subsystem {
 
- private static final int TIMEOUT_MS = 10;
+ private static final int TIMEOUT_MS = 0;
 
  private static final int CPR = 4096;
  private static final double DISTANCE_PER_REV = 0;
+ private static final double MAX_SPEED = 0;
 
  private static final int elevatorIndex = 1;
  private static final double P = 0;
  private static final double I = 0;
  private static final double D = 0;
 
- public boolean isRaising = false;
- public boolean isLowering = false;
+ private boolean isRaising = false;
+ private boolean isLowering = false;
 
  private TalonSRX talon = new TalonSRX(RobotMap.elevatorTalon);
 
@@ -33,6 +35,17 @@ public Elevator() {
  this.talon.config_kP(elevatorIndex, P, TIMEOUT_MS);
  this.talon.config_kI(elevatorIndex, I, TIMEOUT_MS);
  this.talon.config_kD(elevatorIndex, D, TIMEOUT_MS);
+ this.talon.configNominalOutputForward(+0, TIMEOUT_MS);
+ this.talon.configNominalOutputReverse(-0, TIMEOUT_MS);
+ this.talon.configPeakOutputForward(+1, TIMEOUT_MS);
+ this.talon.configPeakOutputReverse(-1, TIMEOUT_MS);
+ this.talon.enableVoltageCompensation(true);
+ this.talon.configOpenloopRamp(2, TIMEOUT_MS);
+ this.talon.configAllowableClosedloopError(elevatorIndex, 1000, TIMEOUT_MS);
+ this.talon.configMotionCruiseVelocity((int) (MAX_SPEED * 0.7), TIMEOUT_MS);
+ this.talon.configMotionAcceleration((int) (MAX_SPEED * .25), TIMEOUT_MS);
+ this.talon.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, TIMEOUT_MS);
+ this.talon.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, TIMEOUT_MS);
  }
 
  @Override
@@ -64,6 +77,13 @@ public void set(double value) {
  }
  }
 
+ public void setPID(double P, double I, double D) {
+ this.talon.selectProfileSlot(elevatorIndex, 0);
+ this.talon.config_kP(elevatorIndex, P, TIMEOUT_MS);
+ this.talon.config_kI(elevatorIndex, I, TIMEOUT_MS);
+ this.talon.config_kD(elevatorIndex, D, TIMEOUT_MS);
+ }
+
  public void zeroEncoder() {
  this.talon.getSensorCollection().setQuadraturePosition(0, TIMEOUT_MS);
  }
@@ -76,6 +96,10 @@ public double getHeight() {
  return this.talon.getSelectedSensorPosition(0) / CPR * DISTANCE_PER_REV + DistanceConstants.LOW_LIMIT;
  }
 
+ public double getSpeed() {
+ return this.talon.getSelectedSensorVelocity(0);
+ }
+
  public void enableBrakeMode() {
  this.talon.setNeutralMode(NeutralMode.Brake);
  }
@@ -87,4 +111,12 @@ public void disableBrakeMode() {
  public void stop() {
  this.set(0);
  }
+
+ public boolean isRaising() {
+ return this.isRaising;
+ }
+
+ public boolean isLowering() {
+ return this.isLowering;
+ }
 }
diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/Vision.java b/src/main/java/com/frcteam1939/powerup2018/robot/subsystems/Vision.java
@@ -9,8 +9,6 @@
 import com.frcteam1939.powerup2018.util.PixyPacket;
 import com.frcteam1939.powerup2018.util.PixySPI;
 
-import edu.wpi.first.wpilibj.PIDController;
-import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.SPI.Port;
 import edu.wpi.first.wpilibj.command.Subsystem;
@@ -49,18 +47,18 @@ public int getX() {
  }
  return this.packets.get(0).get(0).X;
  }
+
  public void center() {
- double error = this.center- this.getX();
- while(error != 0) {
+ double error = this.center - this.getX();
+ while (error != 0) {
  double move = Robot.oi.left.getY();
- if(move <= 0.1) {
+ if (move <= 0.1) {
  move = 0;
  }
- 
- Robot.drivetrain.drive(move, kP*error);
+
+ Robot.drivetrain.drive(move, this.kP * error);
  }
  }
-
 
  public void testPixy1() {
  int ret = -1;