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

Add test mode for finding horizontal comp

Browse Source
This commit is contained in:
HFocus
2019-09-27 23:25:15 -06:00
parent 876be2547e
commit a4eb41e63d
2 changed files with 27 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+1
View File
@@ -141,6 +141,7 @@ public class Robot extends TimedRobot
public void updateStatus() {
arm.updateStatus(operationMode);
arm.readStatus();
drive.updateStatus(operationMode);
wrist.updateStatus(operationMode);
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+26 -5
View File
@@ -53,7 +53,9 @@ public class Arm extends Subsystem implements ControlLoopable
{
private static Arm instance;
public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC, SMART_MOTION, F_SWITCH, R_SWITCH};
public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC, SMART_MOTION, F_SWITCH, R_SWITCH,
JOYSTICK_MANUAL_TEST
};
public static enum PlaceMode { HATCH, CARGO };
// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks
@@ -74,6 +76,8 @@ public class Arm extends Subsystem implements ControlLoopable
public static final double JOYSTICK_INCHES_PER_MS_LO = 35;
public static final double HORIZONTAL_COMPENSATION = 0.0; ///TODO
public static double testOutput = 0;
// Defined positions (from now on only use variables ending with _ROT)
public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
public static final double ZERO_POSITION_INCHES = -0.25;
@@ -244,6 +248,10 @@ public class Arm extends Subsystem implements ControlLoopable
controlManualWithJoystick();
// System.err.println(motorController.getControlMode());
}
if (armControlMode == ArmControlMode.JOYSTICK_MANUAL_TEST) {
controlManualWithJoystickTest();
// System.err.println(motorController.getControlMode());
}
/* else if (armControlMode == ArmControlMode.F_SWITCH){
controlManualWithJoystickFSwitch();
}
@@ -306,14 +314,15 @@ public class Arm extends Subsystem implements ControlLoopable
}
public void setSpeedJoystick(double speed) {
double comp = calcGravityCompensationAtCurrentPosition();
speed = (1 - comp) * calcSoftLimits(speed); ///TODO: Enable for SoftLimits
speed += comp; ///TODO: Enable for Compensation
setSpeed(speed);
setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
}
public void setSpeed(double speed) {
// motor1.set(ControlMode.PercentOutput, speed);
// speed = calcSoftLimits(speed); ///TODO: Enable for SoftLimits
// speed += calcGravityCompensationAtCurrentPosition(); ///TODO: Enable for Compensation
motor1.set(speed);
setArmControlMode(ArmControlMode.MANUAL);
}
@@ -337,7 +346,8 @@ public class Arm extends Subsystem implements ControlLoopable
double rot = motorEncoder.getPosition();
double rad = armRotToRadians(rot);
double compensation = HORIZONTAL_COMPENSATION * Math.sin(rad);
return compensation;
if (MIN_POSITION_ROT < rot && rot < MAX_POSITION_ROT) return compensation;
else return 0;
}
public double armRotToRadians(double rot){
@@ -355,6 +365,13 @@ public class Arm extends Subsystem implements ControlLoopable
setSpeedJoystick(joyStickSpeed);
}
private void controlManualWithJoystickTest() {
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
joyStickSpeed *= testOutput * 0.3;
joyStickSpeed += testOutput;
setSpeed(joyStickSpeed);
}
private void controlManualWithJoystickFSwitch(){
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
if (joyStickSpeed > 0) joyStickSpeed = 0;
@@ -553,6 +570,8 @@ public class Arm extends Subsystem implements ControlLoopable
//SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
//SmartDashboard.putNumber("Arm Target PID Position", targetPositionInchesPID);
SmartDashboard.putNumber("Arm Test Output", testOutput);
// display PID coefficients on SmartDashboard
SmartDashboard.putNumber("Arm P Gain", kP);
SmartDashboard.putNumber("Arm I Gain", kI);
@@ -575,7 +594,7 @@ public class Arm extends Subsystem implements ControlLoopable
}
public void ReadStatus(){
public void readStatus(){
try {
// read PID coefficients from SmartDashboard
double p = SmartDashboard.getNumber("Arm P Gain", 0);
@@ -589,6 +608,7 @@ public class Arm extends Subsystem implements ControlLoopable
double minV = SmartDashboard.getNumber("Arm Min Velocity", 0);
double maxA = SmartDashboard.getNumber("Arm Max Acceleration", 0);
double allE = SmartDashboard.getNumber("Arm Allowed Closed Loop Error", 0);
double testOut = SmartDashboard.getNumber("Arm Test Output", 0);
// if PID coefficients on SmartDashboard have changed, write new values to controller
if((p != kP)) { motorController.setP(p); kP = p; }
@@ -604,6 +624,7 @@ public class Arm extends Subsystem implements ControlLoopable
if((minV != minVel)) { motorController.setSmartMotionMinOutputVelocity(minV,0); minVel = minV; }
if((maxA != maxAcc)) { motorController.setSmartMotionMaxAccel(maxA,0); maxAcc = maxA; }
if((allE != allowedErr)) { motorController.setSmartMotionAllowedClosedLoopError(allE,0); allE = allowedErr; }
if((testOut != testOutput)) { testOutput = testOut; }
// if Set Position has changed, set as target for motorController
double setPos = SmartDashboard.getNumber("Arm Set Position", 0);