Permalink
Browse files

Auto and Port Changes

  • Loading branch information...
Maxcr1 committed Jul 17, 2018
1 parent 09bc7ed commit 970551d3ce044fea418fa213f1d3132bea7ea88e
@@ -16,11 +16,11 @@
// Elevator TalonSRX IDs
public static final int elevatorMasterID = 7;
public static final int elevatorSlaveID = 6;
public static final int armID = 9;
public static final int armID = 10;

// Intake TalonSRX IDs
public static final int leftIntakeID = 8;
public static final int rightIntakeID = 10;
public static final int rightIntakeID = 9;

public static final int hookDeplyID = 11;

@@ -30,7 +30,7 @@

// Intake Piston
public static final int clampPortForward = 5;
public static final int clampPortReverse = 7;
public static final int clampPortReverse = 4;

// Drive Train Shifting
public static final int dtSpeedShiftForward = 1;
@@ -9,7 +9,7 @@
public class Constants {

//Global Constants

// Auto Names
public static final String preferenceOneName = "Switch";
public static final String preferenceTwoName = "Scale";
@@ -42,6 +42,7 @@
public static final double kNeutralDeadband = 0;
public static final int kTimeoutMs = 10;
public static final int kBaseTrajPeriodMs = 0;
public static final double minimumEncoderVoltage = 0;


//Drive Train
@@ -112,22 +113,26 @@ public static void setElevatorDataFromConstants() {


//Elevator Encoder Heights
public static int groundElevatorEncoderPosition = -600;
public static int groundElevatorEncoderPosition = -300;
public static int switchElevatorEncoderPosition = 7750;
public static int scaleLowElevatorEncoderPosition = 9000;
public static int scaleMidElevatorEncoderPosition = 14000;
public static int scaleHighElevatorEncoderPosition = 17750;
public static int scaleHighElevatorEncoderPosition = 19500;

//Ramp Rate
public static final double elevatorRampRate = .75;
public static final double elevatorRampRate = .3;


//Arm

public static int extendedArmEncoderPosition = 3930;
public static int extendedArmEncoderPosition = 3934 - 53;
public static int retractedArmEncoderPosition = extendedArmEncoderPosition - 600;
public static int stowArmEncoderPosition = 215;

public static final double armLowerBound = -1250;
public static final double armUpperBound = 0;


//Arm TalonConfig Data
public static TalonConfig armData = new TalonConfig();
public static void setArmDataFromConstants() {
@@ -155,6 +160,7 @@ public static void setArmDataFromConstants() {
//Climber

public static final double hookDeploySpeed = .5;




@@ -1,8 +1,5 @@
package com.team2169.robot;

import com.team2169.robot.RobotStates.ArmPos;
import com.team2169.robot.RobotStates.DriveType;
import com.team2169.robot.RobotStates.Macro;
import com.team2169.robot.RobotStates.RunningMode;
import com.team2169.robot.auto.AutoManager;
import com.team2169.robot.canCycles.CANCycleHandler;
@@ -69,7 +66,6 @@ public void autonomousPeriodic() {
System.out.println("Auto Error");
}
superStructure.subsystemLooper();
// auto.autoLooping();

}

@@ -81,9 +77,6 @@ public void teleopInit() {
}
System.out.println("Starting Tele");
Scheduler.getInstance().removeAll();
RobotStates.armPos = ArmPos.IDLE;
RobotWantedStates.wantedDriveType = DriveType.NORMAL_DRIVING;
RobotWantedStates.wantedElevatorPos = Macro.PASS;
}

@Override
@@ -5,6 +5,7 @@
public class RobotStates {

public static boolean debugMode = false;
public static int loopCount = 0;

// FMS Handler
static boolean isFMSConnected;
@@ -70,11 +70,11 @@
}

public static class FarScale {
public static double startToPoint = 210;
public static double startToPoint = 215;
public static double pointToPoint2Turn = 90;
public static double pointToPoint2 = 236;
public static double point2ToScaleTurn = -105;
public static double point2ToScale = 65;
public static double point2ToScaleTurn = -110;
public static double point2ToScale = 62;
public static double intakeSpeed = .4;
public static double intakeInSpeed = .5;
}
@@ -88,21 +88,21 @@
public static double pointToScaleTurn = 50;
public static double pointToScale = 17;
public static double pointToBlockTurn = 100;
public static double pointToBlock = 64;
public static double pointToBlock = 61;
public static double intakeSpeed = .7;
public static double intakeInSpeed = .5;
public static double intakeInSpeed = 1;

public static class ScaleSwitchClose {
public static double blockToSwitch = 6;
}
}

public static class Far {
public static double startToPoint = 210;
public static double startToPoint = 215;
public static double pointToPoint2Turn = 90;
public static double pointToPoint2 = 236;
public static double point2ToScaleTurn = -95;
public static double point2ToScale = 65;
public static double point2ToScale = 62;
public static double point2ToPoint3 = -18;
public static double point3ToBlockTurn = -120;
public static double point3ToBlock = 48;
@@ -140,7 +140,7 @@
public static class TwoBlock {

public static double switchToPoint = -70;
public static double pointToPileTurn = 45;
public static double pointToPileTurn = 35;
public static double pointToPile = 52;
}

@@ -1,6 +1,11 @@
package com.team2169.robot.auto;

import com.team2169.robot.Constants;
import com.team2169.robot.RobotStates;
import com.team2169.robot.RobotWantedStates;
import com.team2169.robot.RobotStates.ArmPos;
import com.team2169.robot.RobotStates.DriveType;
import com.team2169.robot.RobotStates.Macro;
import com.team2169.robot.auto.AutoConstants.AutoSequenceMode;
import com.team2169.robot.auto.AutoConstants.CenterPriority;
import com.team2169.robot.auto.AutoConstants.Possesion;
@@ -153,6 +158,9 @@ public void endAuto() {
if (auto != null) {
auto.cancel();
}
RobotStates.armPos = ArmPos.IDLE;
RobotWantedStates.wantedDriveType = DriveType.NORMAL_DRIVING;
RobotWantedStates.wantedElevatorPos = Macro.PASS;
}

public void autoLooping() {
@@ -90,7 +90,7 @@ public TwoBlockScaleCloseAuto(RobotSide side) {

//Clamp block, wait 1/4 of a second and start to bring elevator up
addSequential(new IntakeClampAction());
addSequential(new WaitCommand(.25));
addSequential(new WaitCommand(.5));

//Back up to scale and stop the intakes
addParallel(new ElevatorToScaleHigh());
@@ -12,6 +12,7 @@
private Arm arm;
private PIDF pid;
private double setpoint;
private int errorCounter;

public ArmPID(Arm arm_) {

@@ -39,8 +40,23 @@ public void loop() {
pid.setI(Constants.armData.i);
pid.setD(Constants.armData.d);
pid.setF(Constants.armData.f);
double output = pid.getOutput(getPosition());
arm.arm.set(ControlMode.PercentOutput, output);

if(Math.abs(getPosition() - setpoint) > 7) {
errorCounter++;
}
else {
errorCounter = 0;
}

if(errorCounter > 4) {
double output = pid.getOutput(getPosition());
arm.arm.set(ControlMode.PercentOutput, output);
}
else {
arm.arm.set(ControlMode.PercentOutput, 0);
}


}

public int getPosition() {
@@ -82,7 +82,7 @@ public void armMacroLooper() {
}

public int getEncPosition() {
return enc.getValue();
return enc.getAverageValue();
}

private void runToPosition(int pos) {
@@ -217,7 +217,8 @@ public TalonSRX prepElevatorTalon(TalonSRX talon) {
talon.configPeakCurrentLimit(35, Constants.elevatorUpData.timeoutMs);
talon.configPeakCurrentDuration(500, Constants.elevatorUpData.timeoutMs);
talon.setSensorPhase(false);
talon.configClosedloopRamp(Constants.elevatorRampRate, 10);
talon.configClosedloopRamp(.5, 10);
talon.configOpenloopRamp(Constants.elevatorRampRate, 10);
return talon;

}
@@ -112,6 +112,7 @@ else if(ControlMap.intakeAmount() <= -ControlMap.operatorDeadband) {
// Set Clamp to Clamped
clampSolenoid.set(Value.kReverse);
RobotStates.intakeClamp = IntakeClamp.CLAMP;
lastIntook = true;
break;

case OPEN:
@@ -11,7 +11,6 @@

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Superstructure {

@@ -68,19 +67,19 @@ public void startCompressor() {
comp.start();
}

public void subsystemLooper() {
public void subsystemLooper() {

DataSelector.looper();

SmartDashboard.putNumber("Voltage", DriverStation.getInstance().getBatteryVoltage());

drive.driveHandler();
intake.intakeHandler();
liftArm.elevatorArmHandler();
climber.climberHandler();

smartDashboardLooper();

RobotStates.loopCount++;

}

private void smartDashboardLooper() {
Oops, something went wrong.

0 comments on commit 970551d

Please sign in to comment.