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

added pid to wrist and arm,

Browse Source 
arm pid "works" wrist is being worked on
This commit is contained in:
lukesta182
2019-03-08 18:46:38 -07:00
parent cfcbf2ce4a
commit f74093ca12
5 changed files with 90 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+4 -2
View File
@@ -98,7 +98,8 @@ public class Robot extends TimedRobot
drive.endGyroCalibration();
drive.resetEncoders();
drive.resetGyro();
drive.setIsRed(getAlliance().equals(Alliance.Red));
drive.setIsRed(getAlliance().equals(Alliance.Red));
arm.resetencoder();
}
@@ -147,7 +148,8 @@ public class Robot extends TimedRobot
public void updateStatus() {
arm.updateStatus(operationMode);
drive.updateStatus(operationMode);
drive.updateStatus(operationMode);
wrist.updateStatus(operationMode);
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java
+1 -1
View File
@@ -14,7 +14,7 @@ public class ArmTest extends CommandGroup {
* Add your docs here.
*/
public ArmTest() {
addSequential(new ArmSetPositionPID(1110));
addSequential(new ArmSetPositionPID(1400));
// Add Commands here:
// e.g. addSequential(new Command1());
// addSequential(new Command2());
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionPID.java
+42
View File
@@ -0,0 +1,42 @@
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.subsystems.Wrist;
import edu.wpi.first.wpilibj.command.Command;
/**
*
*/
public class WristSetPositionPID extends Command {
private double targetPositionInches;
public WristSetPositionPID(double targetPositionInches) {
this.targetPositionInches = targetPositionInches;
requires(Robot.wrist);
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.wrist.setPositionPID(targetPositionInches);
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return Math.abs(Robot.wrist.getPositionInches() - this.targetPositionInches) < Wrist.PID_ERROR_INCHES;
}
// Called once after isFinished returns true
protected void end() {
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+24 -15
View File
@@ -44,8 +44,8 @@ public class Arm extends Subsystem implements ControlLoopable
public static final double TEST_SPEED_UP = 0.3;
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 = 0.75;
public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
public static final double JOYSTICK_INCHES_PER_MS_HI = 20;
public static final double JOYSTICK_INCHES_PER_MS_LO = 20;
// Defined positions
public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
@@ -84,13 +84,14 @@ public class Arm extends Subsystem implements ControlLoopable
private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);
private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);
public static final double KF_UP = 0.06;
public static final double KF_DOWN = 0.01;
public static final double P_Value = 0.6;
public static final double I_Value = 0.0005;
public static final double D_Value = 0.0;
public static final double KF_UP = 0.01;
public static final double KF_DOWN = 0.0;
public static final double P_Value = 2;
public static final double I_Value = 0.00300;
public static final double D_Value = 200;
public static final double RampRate = 0.0;
private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later
public static final double PID_ERROR_INCHES = 1.0;
public static final double PID_ERROR_INCHES = 5.0;
LimitSwitchSource limitSwitchSource;
// Pneumatics
private Solenoid speedShift;
@@ -117,11 +118,12 @@ public class Arm extends Subsystem implements ControlLoopable
// if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
// Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
// }
motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
motor1.setNeutralMode(NeutralMode.Brake);
motor2.setNeutralMode(NeutralMode.Brake);
motor1.enableCurrentLimit(true);
motorControllers.add(motor1);
SmartDashboard.putNumber("arm P value", 0);
@@ -138,6 +140,9 @@ public class Arm extends Subsystem implements ControlLoopable
public void resetZeroPosition(double position) {
mpController.resetZeroPosition(position);
}
public void resetencoder(){
motor1.setPosition(0);
}
private synchronized void setArmControlMode(ArmControlMode controlMode) {
this.armControlMode = controlMode;
@@ -168,12 +173,16 @@ public class Arm extends Subsystem implements ControlLoopable
public void updatePositionPID(double targetPositionInches) {
targetPositionInchesPID = limitPosition(targetPositionInches);
double startPositionInches = motor1.getPositionWorld();
mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
//motor1.set(ControlMode.Position, targetPositionInches);
//motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS);
//motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS);
//motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS);
System.err.println(motor1.getControlMode());
//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
motor1.set(ControlMode.Position, targetPositionInches);
motor1.configClosedloopRamp(.02);
//motor1.configPeakCurrentLimit(5);
motor1.configContinuousCurrentLimit(2);
motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kD(0, D_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS);
//System.err.println(motor1.getControlMode());
//System.err.print(motor1.getClosedLoopError());
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+19 -5
View File
@@ -83,9 +83,14 @@ public class Wrist extends Subsystem implements ControlLoopable
private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);
private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);
public static final double KF_UP = 0.005;
public static final double KF_DOWN = 0.0;
public static final double PID_ERROR_INCHES = 1.0;
public static final double WristKF_UP = 0.01;
public static final double WristKF_DOWN = 0.0;
public static final double WristP_Value = 2;
public static final double WristI_Value = 0.00300;
public static final double WristD_Value = 200;
public static final double WristRampRate = 0.0;
public static final double PID_ERROR_INCHES = 10;
LimitSwitchSource limitSwitchSource;
// Pneumatics
private Solenoid speedShift;
@@ -156,9 +161,18 @@ public class Wrist extends Subsystem implements ControlLoopable
}
public void updatePositionPID(double targetPositionInches) {
targetPositionInchesPID = limitPosition(targetPositionInches);
targetPositionInchesPID = limitPosition(targetPositionInches);
double startPositionInches = wristmotor1.getPositionWorld();
mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
wristmotor1.set(ControlMode.Position, targetPositionInches);
wristmotor1.configClosedloopRamp(WristRampRate);
//motor1.configPeakCurrentLimit(5);
wristmotor1.configClosedloopRamp(1);
wristmotor1.configContinuousCurrentLimit(2);
wristmotor1.config_kP(0, WristP_Value, TalonSRXEncoder.TIMEOUT_MS);
wristmotor1.config_kI(0, WristI_Value, TalonSRXEncoder.TIMEOUT_MS);
wristmotor1.config_kD(0, WristD_Value, TalonSRXEncoder.TIMEOUT_MS);
wristmotor1.config_kF(0, targetPositionInchesPID > startPositionInches ? WristKF_UP : WristKF_DOWN, TalonSRXEncoder.TIMEOUT_MS);
}
public void setPositionMP(double targetPositionInches) {