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

Controling flip for wrist

Browse Source 
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
This commit is contained in:
mayabartels
2019-02-01 15:31:22 -08:00
parent 56f674578b
commit 11220abce3
1 changed files with 26 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+26 -6
View File
@@ -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<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
@@ -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()
{