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

Set up Smart Motion Control

Browse Source 
- All thats left is implementing Smart Motion with Rev Robotics libraries
This commit is contained in:
HFocus
2019-09-08 18:58:16 -06:00
parent f565a82836
commit 9fbe77138b
1 changed files with 27 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+27 -4
View File
@@ -116,9 +116,6 @@ public class Arm extends Subsystem implements ControlLoopable
public static double kMinOutput = -1;
public static double kMaxOutput = 1;
private PIDParams armPIDParams = new PIDParams(kP, kI, kD, kFF_Down); // KF gets updated later
public static final double PID_ERROR_INCHES = 50;
LimitSwitchSource limitSwitchSource;
@@ -226,7 +223,7 @@ public class Arm extends Subsystem implements ControlLoopable
// System.err.println(motor1.getControlMode());
}
else if (armControlMode == ArmControlMode.SMART_MOTION){
controlSMWithJoystick();
}
/* if (armControlMode == ArmControlMode.MOTION_PROFILE) {
//isFinished = mpController.controlLoopUpdate(getPositionInches());
@@ -342,6 +339,31 @@ public class Arm extends Subsystem implements ControlLoopable
// System.err.print(motor1.getClosedLoopError());
}
private void controlSMWithJoystick() {
double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
double deltaPosition = joystickPosition * joystickInchesPerMs;
targetPositionInchesSM = targetPositionInchesSM + deltaPosition;
updatePositionSM(targetPositionInchesSM);
}
public void setPositionSM(double targetPositionInches) {
// motorController.setReference(targetPositionInches, ControlType.kSmartMotion);
updatePositionSM(targetPositionInches);
setArmControlMode(ArmControlMode.SMART_MOTION);
setFinished(false);
}
public void updatePositionSM(double targetPositionInches) {
targetPositionInchesSM = limitPosition(targetPositionInches);
if (limitPosition(motorEncoder.getPosition()) == MIN_POSITION_INCHES){
resetEncoder();
}
double startPositionInches = motorEncoder.getPosition();
motorController.setReference(targetPositionInches, ControlType.kSmartMotion);
motorController.setFF(targetPositionInchesPID > startPositionInches ? kFF_Up : kFF_Down);
}
private double limitPosition(double targetPosition) {
if (targetPosition < MIN_POSITION_INCHES) {
return MIN_POSITION_INCHES;
@@ -458,6 +480,7 @@ public class Arm extends Subsystem implements ControlLoopable
//SmartDashboard.putNumber("Arm Error", motor1.getClosedLoopError());
SmartDashboard.putNumber("Arm Amps", getAverageMotorCurrent());
SmartDashboard.putNumber("Arm Target SM", targetPositionInchesSM);
SmartDashboard.putString("Arm Control Mode", armControlMode.toString());
//SmartDashboard.putNumber("arm output", motor1.getMotorOutputPercent());
//SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID));
//SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));