Team4388/2019-Hit-or-Miss
1
0
Fork 0
mirror of https://github.com/Team4388/2019-Hit-or-Miss.git synced 2026-06-09 00:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

final changes befor denver

Browse Source 
lets get this bread
mm on wrist, height claibrations
This commit is contained in:
lukesta182
2019-03-19 23:10:58 -06:00
parent 4897484183
commit 52551db134
10 changed files with 39 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+1 -1
View File
@@ -112,7 +112,7 @@ public class Robot extends TimedRobot
//drive.resetEncoders();
drive.endGyroCalibration();
arm.targetPositionInchesMM = arm.motor1.getPositionWorld();
wrist.targetPositionInchesPID = wrist.wristMotor1.getPositionWorld();
wrist.targetPositionInchesMM = wrist.wristMotor1.getPositionWorld();
updateStatus();
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionMM.java
+1 -2
View File
@@ -22,8 +22,7 @@ public class WristSetPositionMM extends Command {
protected void initialize() {
if (Math.abs(targetPositionInches - Robot.wrist.getPositionInches()) < MIN_DELTA_TARGET) {
isAtTarget = true;
System.out.println("Wrist is at target");
isAtTarget = true;
}
else {
isAtTarget = false;
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java
+1 -1
View File
@@ -21,7 +21,7 @@ public class CargoHigh extends CommandGroup {
addSequential(new HatchFlip(false));
addParallel(new setWrist(2781));
addParallel(new DelayHatch());
addSequential(new ArmSetPositionMM(3812));
addSequential(new ArmSetPositionMM(4038));
// Add Commands here:
// e.g. addSequential(new Command1());
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java
+1 -1
View File
@@ -18,7 +18,7 @@ public class CargoLow extends CommandGroup {
*/
public CargoLow() {
addParallel(new setWrist(2300));
addParallel(new setWrist(2200));
addSequential(new ArmSetPositionMM(1300));
// Add Commands here:
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java
+2 -2
View File
@@ -18,8 +18,8 @@ public class CargoMid extends CommandGroup {
*/
public CargoMid() {
addParallel(new setWrist(2430));
addSequential(new ArmSetPositionMM(2630));
addParallel(new setWrist(2330));
addSequential(new ArmSetPositionMM(2580));
// Add Commands here:
// e.g. addSequential(new Command1());
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java
+2 -2
View File
@@ -19,9 +19,9 @@ public class HatchHigh extends CommandGroup {
*/
public HatchHigh() {
addSequential(new HatchFlip(false));
addParallel(new setWrist(1144));
addParallel(new setWrist(852));
addParallel(new DelayHatch());
addSequential(new ArmSetPositionMM(3605));
addSequential(new ArmSetPositionMM(3451));
// Add Commands here:
// e.g. addSequential(new Command1());
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java
+1 -1
View File
@@ -18,7 +18,7 @@ public class HatchLow extends CommandGroup {
*/
public HatchLow() {
addParallel(new setWrist(350));
addParallel(new setWrist(200));
addSequential(new ArmSetPositionMM(550));
// Add Commands here:
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java
+2 -2
View File
@@ -18,8 +18,8 @@ public class HatchMid extends CommandGroup {
*/
public HatchMid() {
addParallel(new setWrist(750));
addSequential(new ArmSetPositionMM(2000));
addParallel(new setWrist(525));
addSequential(new ArmSetPositionMM(2050));
// Add Commands here:
// e.g. addSequential(new Command1());
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/setWrist.java
+2 -1
View File
@@ -7,6 +7,7 @@
package org.usfirst.frc4388.robot.commands.presets;
import org.usfirst.frc4388.robot.commands.WristSetPositionMM;
import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -18,7 +19,7 @@ public class setWrist extends CommandGroup {
*/
public setWrist(double wrist) {
addSequential(new WaitCommand(1));
addSequential(new WristSetPositionPID(wrist));
addSequential(new WristSetPositionMM(wrist));
// Add Commands here:
// addSequential(new Command2());
// these will run in order.
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+26 -36
View File
@@ -46,7 +46,7 @@ public class Wrist extends Subsystem implements ControlLoopable
public static final double TEST_SPEED_DOWN = -0.3;
public static final double AUTO_ZERO_SPEED = -0.3;
public static final double JOYSTICK_INCHES_PER_MS_HI = 20;
public static final double JOYSTICK_INCHES_PER_MS_LO = 27;
public static final double JOYSTICK_INCHES_PER_MS_LO = 20;
// Defined positions
public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
@@ -87,12 +87,12 @@ public class Wrist extends Subsystem implements ControlLoopable
private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);
public static final double KF_UP = 0.01;
public static final double KF_DOWN = 0.0;
public static final double P_Value = 1;
public static final double I_Value = 0.0000;
public static final double D_Value = 000;
public static final double P_Value = 1.2;
public static final double I_Value = 0.002;
public static final double D_Value = 125;
public static final double F_Value = 1;
public static final int CV_value = 100;
public static final int A_value = 100;
public static final int CV_value = 200;
public static final int A_value = 350;
public static final double RampRate = 0.0;
public static final double maxGravityComp = 0;
private PIDParams wristPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later
@@ -104,7 +104,7 @@ public class Wrist extends Subsystem implements ControlLoopable
// Misc
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
private boolean isFinished;
private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID;
private WristControlMode wristControlMode = WristControlMode.MOTION_MAGIC;
public double targetPositionInchesPID = 0;
public double targetPositionInchesMM = 0;
private boolean firstMpPoint;
@@ -122,12 +122,6 @@ public class Wrist extends Subsystem implements ControlLoopable
// Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
// }
wristMotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
wristMotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
wristMotor1.setNeutralMode(NeutralMode.Brake);
wristMotor1.enableCurrentLimit(true);
//wristMotor1.setSensorPhase(true);
wristMotor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS);
wristMotor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS);
wristMotor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS);
@@ -138,6 +132,12 @@ public class Wrist extends Subsystem implements ControlLoopable
wristMotor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS);
wristMotor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS);
wristMotor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS);
wristMotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
wristMotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
wristMotor1.setNeutralMode(NeutralMode.Brake);
wristMotor1.enableCurrentLimit(true);
//wristMotor1.setSensorPhase(true);
motorControllers.add(wristMotor1);
}
catch (Exception e) {
@@ -175,11 +175,11 @@ public class Wrist extends Subsystem implements ControlLoopable
wristMotor1.set(ControlMode.PercentOutput, speed);
setWristControlMode(WristControlMode.JOYSTICK_MANUAL);
}
public void setPositionMM(double targetPositionInches){
wristMotor1.selectProfileSlot(MM_SLOT, 0);
wristMotor1.set(ControlMode.MotionMagic, targetPositionInches);
//System.err.println(wristMotor1.getControlMode());
wristMotor1.selectProfileSlot(MM_SLOT, 0);
setWristControlMode(WristControlMode.MOTION_MAGIC);
updatePositionMM(targetPositionInches);
setFinished(false);
@@ -219,7 +219,7 @@ public class Wrist extends Subsystem implements ControlLoopable
double startPositionInches = wristMotor1.getPositionWorld();
//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
wristMotor1.set(ControlMode.Position, targetPositionInches);
wristMotor1.configClosedloopRamp(.02);
wristMotor1.configClosedloopRamp(.02); //TODO: Arm has this set to zero
//wristMotor1.configPeakCurrentLimit(5);
wristMotor1.configContinuousCurrentLimit(2);
wristMotor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS);
@@ -318,23 +318,20 @@ public class Wrist extends Subsystem implements ControlLoopable
// Do the update
if (wristControlMode == WristControlMode.JOYSTICK_MANUAL) {
controlManualWithJoystick();
}
else if (!isFinished) {
if (wristControlMode == WristControlMode.MOTION_PROFILE) {
isFinished = mpController.controlLoopUpdate(getPositionInches());
}
if (wristControlMode == WristControlMode.JOYSTICK_PID){
controlPidWithJoystick();
//System.err.println(wristMotor1.getControlMode());
}
if (wristControlMode == WristControlMode.MOTION_MAGIC){
controlMMWithJoystick();
//System.err.println(wristMotor1.getControlMode());
}
if (wristControlMode == WristControlMode.JOYSTICK_PID){
//System.err.println(wristMotor1.getControlMode());
controlPidWithJoystick();
}
/*else if (wristControlMode == WristControlMode.MP_PATH_VELOCITY) {
isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
@@ -347,25 +344,18 @@ public class Wrist extends Subsystem implements ControlLoopable
}
private void controlPidWithJoystick() {
double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis();
double deltaPosition = joystickPosition * joystickInchesPerMs;
targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
updatePositionPID(targetPositionInchesPID);
}
private void controlMMWithJoystick() {
double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis();
double deltaPosition = joystickPosition * joystickInchesPerMs;
targetPositionInchesMM = targetPositionInchesMM + deltaPosition;
updatePositionMM(targetPositionInchesMM);
//Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3);
}
private void controlPidWithJoystick() {
double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis();
double deltaPosition = joystickPosition * joystickInchesPerMs;
targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
updatePositionPID(targetPositionInchesPID);
}
private void ControlWithJoystickhold(){