From 56f674578b46c70620dcc70a7f1998a47782d5c7 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Fri, 1 Feb 2019 15:08:46 -0800 Subject: [PATCH 1/4] Additions to PID loop in wrist --- .../java/org/usfirst/frc4388/robot/OI.java | 2 ++ .../frc4388/robot/subsystems/Wrist.java | 18 ++++++++++++++++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index e2a4baa..d2d6d52 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -65,6 +65,8 @@ public class OI JoystickButton ArmAimAssist = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON); ArmAimAssist.whenPressed(new ArmSetMode(ArmControlMode.PID)); + + // uncoment the line above diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index 9d977c5..838ed9f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -15,6 +15,8 @@ import org.usfirst.frc4388.utility.CANTalonEncoder; import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.PIDParams; import org.usfirst.frc4388.utility.SoftwarePIDPositionController; +import org.usfirst.frc4388.utility.MPTalonPIDPathController; +import org.usfirst.frc4388.utility.MPTalonPIDController; import org.usfirst.frc4388.robot.subsystems.Arm; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -51,11 +53,11 @@ public class Wrist extends Subsystem public static int MP_SLOT = 1; 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); - private PIDParams pidPIDParamsLoGear = new PIDParams(0.45, 0.0, 0.0, 0.0, 0.0, 0.0); + private PIDParams pidPIDParamsLevel = 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; + private long periodMs = (long)(Constants.kLooperDt * 1000.0); // Defined positions public static final double MIN_POSITION_INCHES = 0.0; @@ -154,6 +156,13 @@ public class Wrist extends Subsystem setWristControlMode(wristControlMode.JOYSTICK_MANUAL); } + public void onStart(double timestamp) + { + //mpController.setPID(mpPIDParams); + mpController.setPID(pidPIDParamsLevel); + mpController.setPIDSlot(PID_SLOT); + } + //@Override public void onLoop(double timestamp) { @@ -199,6 +208,11 @@ public class Wrist extends Subsystem this.isFinished = isFinished; } + public double getPeriodMs() + { + return periodMs; + } + @Override public void initDefaultCommand() { From 11220abce382cd211321dd3768f43516b67796f9 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Fri, 1 Feb 2019 15:31:22 -0800 Subject: [PATCH 2/4] Controling flip for wrist added control mode, changed encoder ticks to degrees, added method for controlling the flip. The angle may need to be adjusted depending on the mechanical design --- .../frc4388/robot/subsystems/Wrist.java | 32 +++++++++++++++---- 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index 838ed9f..b2ba40f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -32,10 +32,11 @@ import com.ctre.phoenix.motorcontrol.ControlMode; /** * Add your docs here. */ + public class Wrist extends Subsystem { //Control Mode Array - public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL}; + public static enum WristControlMode {PID, JOYSTICK_MANUAL, FLIP, GRAB_BALL}; //Motor Controllers private ArrayList motorControllers = new ArrayList(); @@ -43,8 +44,7 @@ public class Wrist extends Subsystem private CANTalonEncoder wristRight; //Encoder ticks to inches for encoders - public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree; - public static final double ENCODER_TICKS_TO_DEGREES = Constants.kWristEncoderTicksPerDegree * Math.PI; + public static final double WRIST_ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3)); // PID controller and params private MPTalonPIDController mpController; @@ -86,7 +86,7 @@ public class Wrist extends Subsystem try { //PID wrist encoder and talon - wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); + wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); } catch(Exception e) { @@ -94,6 +94,14 @@ public class Wrist extends Subsystem } } + //Flipping the intake to the other side + public void flipIntake() + { + double currentWristAngle = wristRight.getPositionWorld(); + + + } + //Method for setting the control mode for the wrist private synchronized void setWristControlMode(WristControlMode controlMode) { @@ -173,7 +181,10 @@ public class Wrist extends Subsystem break; case JOYSTICK_MANUAL: controlManualWithJoystick(); - break; + break; + case FLIP: + controlPIDFlipIntake(); + break; default: break; } @@ -196,7 +207,16 @@ public class Wrist extends Subsystem joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); setSpeedJoystick(joystickSpeed); - } + } + + private void controlPIDFlipIntake() + { + double currentWristAngle = wristRight.getPositionWorld(); + double targetFlipAngle = currentWristAngle - 180; + //Flip angle may need to be adjusted if angle shouldn't be 180 + + updatePositionPID(targetFlipAngle); + } public synchronized boolean isFinished() { From b937837046165453fa1f88cd679d08df63d337d8 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Fri, 1 Feb 2019 15:32:39 -0800 Subject: [PATCH 3/4] Nothing --- .../main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index b2ba40f..b3705c8 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -214,7 +214,7 @@ public class Wrist extends Subsystem double currentWristAngle = wristRight.getPositionWorld(); double targetFlipAngle = currentWristAngle - 180; //Flip angle may need to be adjusted if angle shouldn't be 180 - + updatePositionPID(targetFlipAngle); } From 52ba717ffcb0b2c52e6ed03987344cd82f852179 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Fri, 1 Feb 2019 18:29:10 -0800 Subject: [PATCH 4/4] PID adds Also, joystick for wrist changed from left to right --- .../java/org/usfirst/frc4388/robot/OI.java | 3 +- .../frc4388/robot/subsystems/Wrist.java | 38 +++++++++++++++---- 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index d2d6d52..3d01ef0 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -62,11 +62,10 @@ public class OI JoystickButton wristManualMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON); wristManualMode.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL)); + //Arm JoystickButton ArmAimAssist = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON); ArmAimAssist.whenPressed(new ArmSetMode(ArmControlMode.PID)); - - // uncoment the line above diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index b3705c8..4ee0897 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -36,7 +36,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; public class Wrist extends Subsystem { //Control Mode Array - public static enum WristControlMode {PID, JOYSTICK_MANUAL, FLIP, GRAB_BALL}; + public static enum WristControlMode {PID, JOYSTICK_MANUAL, FLIP_INTAKE, GRAB_BALL}; //Motor Controllers private ArrayList motorControllers = new ArrayList(); @@ -52,7 +52,7 @@ public class Wrist extends Subsystem public static int PID_SLOT = 0; public static int MP_SLOT = 1; - private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); + ///private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); private PIDParams pidPIDParamsLevel = 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; @@ -71,6 +71,14 @@ public class Wrist extends Subsystem public static final double JOYSTICK_Degrees_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; public double armAngleDegrees = Robot.arm.ARM_ANGLE_DEGREES; + + public static final double targetAngleDegreesBallIn = -45; ///Change values + public static final double targetAngleDegreesBallOut = 360; + public static final double targetAngleDegreesHatchIn = 55; + public static final double targetAngleDegreesHatchOut = 0; + + public static final double jumpBarAngle = 50; //hard limit switch? + public static final double armAngleForPIDSwitch = -45; ///Change values //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; @@ -176,15 +184,24 @@ public class Wrist extends Subsystem { synchronized (Wrist.this) { switch(getWristControlMode() ) { - case PID: + case PID: + if(armAngleDegrees > armAngleForPIDSwitch) + { + controlPID(); + } + else if(armAngleDegrees <= armAngleForPIDSwitch) + { + } controlPID(); break; case JOYSTICK_MANUAL: controlManualWithJoystick(); break; - case FLIP: + /* + case FLIP_INTAKE: controlPIDFlipIntake(); break; + */ default: break; } @@ -200,7 +217,7 @@ public class Wrist extends Subsystem updatePositionPID(targetAngleDegreesPID); } - //Method for controlling the motor with the joystick + //Controlling the motor with the joystick private void controlManualWithJoystick() { double joystickSpeed; @@ -209,15 +226,22 @@ public class Wrist extends Subsystem setSpeedJoystick(joystickSpeed); } - private void controlPIDFlipIntake() + //Flip the intake from hatch to ball and visa versa + public void controlPIDFlipIntake() { double currentWristAngle = wristRight.getPositionWorld(); double targetFlipAngle = currentWristAngle - 180; //Flip angle may need to be adjusted if angle shouldn't be 180 - + updatePositionPID(targetFlipAngle); } + //Controlling the PID for the intake going into the robot with ball side facing in + public void controlPIDBallIn() + { + updatePositionPID(targetAngleDegreesBallIn); + } + public synchronized boolean isFinished() { return isFinished;