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 limit switch code

Browse Source 
- Add limit switch code
- Switch from 2 motor arm to 1 motor arm
- Finalize drive train
This commit is contained in:
HFocus
2019-09-27 19:36:18 -06:00
parent 2b4b5e1e0e
commit 414e1c24b3
4 changed files with 53 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java
+4
View File
@@ -39,4 +39,8 @@ public class RobotMap {
public static final int OPEN_INTAKE_PCM_ID = 4;
public static final int CLOSE_INTAKE_PCM_ID = 5;
//DIO
public static final int ARM_LIMIT_FORWARD_ID = 0;
public static final int ARM_LIMIT_REVERSE_ID = 1;
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+44 -14
View File
@@ -53,7 +53,7 @@ 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};
public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC, SMART_MOTION, F_SWITCH, R_SWITCH};
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
@@ -87,11 +87,12 @@ public class Arm extends Subsystem implements ControlLoopable
// Motor controllers
public CANSparkMax motor1;
private CANSparkMax motor2;
//private CANSparkMax motor2;
private CANPIDController motorController;
private CANEncoder motorEncoder;
private CANDigitalInput motorForwardLimitSwitch, motorReverseLimitSwitch;
private DigitalInput armForwardLimit, armReverseLimit;
// PID controller and params
//private MPTalonPIDController mpController;
@@ -151,14 +152,14 @@ public class Arm extends Subsystem implements ControlLoopable
// motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID);
motor1 = new CANSparkMax(RobotMap.ARM_MOTOR1_ID, MotorType.kBrushless);
motor2 = new CANSparkMax(RobotMap.ARM_MOTOR2_ID, MotorType.kBrushless);
//motor2 = new CANSparkMax(RobotMap.ARM_MOTOR2_ID, MotorType.kBrushless);
motor1.restoreFactoryDefaults();
motor2.restoreFactoryDefaults();
motor2.follow(motor1);
//motor2.restoreFactoryDefaults();
//motor2.follow(motor1);
motorController = motor1.getPIDController();
motorEncoder = motor1.getEncoder();
motorForwardLimitSwitch = motor1.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
motorReverseLimitSwitch = motor1.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
//motorEncoder = motor1.getEncoder();
//motorForwardLimitSwitch = motor1.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
//motorReverseLimitSwitch = motor1.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
motorController.setP(kP);
motorController.setI(kI);
@@ -173,6 +174,8 @@ public class Arm extends Subsystem implements ControlLoopable
motorController.setSmartMotionMaxAccel(maxAcc, smartMotionSlot);
motorController.setSmartMotionAllowedClosedLoopError(allowedErr, smartMotionSlot);
armForwardLimit = new DigitalInput(RobotMap.ARM_LIMIT_FORWARD_ID);
armReverseLimit = new DigitalInput(RobotMap.ARM_LIMIT_REVERSE_ID);
// motor1.setInverted(true);
// motor2.setInverted(true);
@@ -217,15 +220,28 @@ public class Arm extends Subsystem implements ControlLoopable
dPadButtons();
if (motorReverseLimitSwitch.get()){
resetEncoder();
if (armForwardLimit.get() && armControlMode == ArmControlMode.JOYSTICK_MANUAL){
setArmControlMode(ArmControlMode.F_SWITCH);
}
else if (armReverseLimit.get() && armControlMode == ArmControlMode.JOYSTICK_MANUAL){
setArmControlMode(ArmControlMode.R_SWITCH);
}
/* if (motorReverseLimitSwitch.get()){
resetEncoder();
} */
// Do the update
if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) {
controlManualWithJoystick();
// System.err.println(motorController.getControlMode());
}
else if (armControlMode == ArmControlMode.F_SWITCH){
controlManualWithJoystickFSwitch();
}
else if (armControlMode == ArmControlMode.R_SWITCH){
controlManualWithJoystickRSwitch();
}
//else if (!isFinished) {
else {
if (armControlMode == ArmControlMode.JOYSTICK_PID){
@@ -288,7 +304,7 @@ public class Arm extends Subsystem implements ControlLoopable
public void setSpeed(double speed) {
// motor1.set(ControlMode.PercentOutput, speed);
motorController.setReference(speed, ControlType.kVoltage);
motor1.set(speed);
setArmControlMode(ArmControlMode.MANUAL);
}
@@ -310,6 +326,20 @@ public class Arm extends Subsystem implements ControlLoopable
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
setSpeedJoystick(joyStickSpeed);
}
private void controlManualWithJoystickFSwitch(){
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
if (joyStickSpeed > 0) joyStickSpeed = 0;
setSpeedJoystick(joyStickSpeed);
if (!armForwardLimit.get()) setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
}
private void controlManualWithJoystickRSwitch(){
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
if (joyStickSpeed < 0) joyStickSpeed = 0;
setSpeedJoystick(joyStickSpeed);
if (!armReverseLimit.get()) setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
}
private void controlPidWithJoystick() {
double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
@@ -388,7 +418,7 @@ public class Arm extends Subsystem implements ControlLoopable
}
public double getAverageMotorCurrent() {
return (motor1.getOutputCurrent() + motor2.getOutputCurrent()) / 2;
return (motor1.getOutputCurrent() /*+ motor2.getOutputCurrent()*/) / 2;
}
public synchronized boolean isFinished() {
@@ -480,7 +510,7 @@ public class Arm extends Subsystem implements ControlLoopable
//System.err.println("the encoder is right after this");
try {
SmartDashboard.putNumber("Arm Ticks", motorEncoder.getPosition());
///SmartDashboard.putNumber("Arm Ticks", motorEncoder.getPosition());
//SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent());
//SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent());
//SmartDashboard.putNumber("sensor vel", motor1.getSelectedSensorVelocity());
@@ -489,7 +519,7 @@ public class Arm extends Subsystem implements ControlLoopable
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("arm output", motor1.getOutputCurrent());
//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));
//SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
+2 -2
View File
@@ -266,7 +266,7 @@ public class Drive extends Subsystem implements ControlLoopable
//rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
//rightDrive1.setNominalClosedLoopVoltage(12.0);
rightDrive1.clearFaults();
rightDrive1.setInverted(true);//false on comp, false on practice
rightDrive1.setInverted(false);//false on comp, false on practice
//rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
//rightDrive1.setSafetyEnabled(false);
rightDrive1.setIdleMode(IdleMode.kBrake);
@@ -279,7 +279,7 @@ public class Drive extends Subsystem implements ControlLoopable
// }
rightDrive2.setInverted(false);//True on comp 2019, false on practice
rightDrive2.setInverted(true);//True on comp 2019, false on practice
//rightDrive2.setSafetyEnabled(false);
rightDrive2.setIdleMode(IdleMode.kBrake);
//rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID());
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+3 -2
View File
@@ -104,7 +104,7 @@ public class Wrist extends Subsystem implements ControlLoopable
// Misc
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
private boolean isFinished;
private WristControlMode wristControlMode = WristControlMode.MOTION_MAGIC;
private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL;
public double targetPositionInchesPID = 0;
public double targetPositionInchesMM = 0;
private boolean firstMpPoint;
@@ -172,7 +172,7 @@ public class Wrist extends Subsystem implements ControlLoopable
}
public void setSpeedJoystick(double speed) {
wristMotor1.set(ControlMode.PercentOutput, speed);
wristMotor1.set(ControlMode.PercentOutput, speed * SPEED_VOLTAGE_RATIO);
setWristControlMode(WristControlMode.JOYSTICK_MANUAL);
}
public void setPositionMM(double targetPositionInches){
@@ -422,6 +422,7 @@ public class Wrist extends Subsystem implements ControlLoopable
// SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));
// SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
SmartDashboard.putNumber("Wrist Target PID", targetPositionInchesPID);
SmartDashboard.putString("Wrist Mode", wristControlMode.toString());
}
catch (Exception e) {
System.err.println("Drivetrain update status error" +e.getMessage());