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

The One Commit to Rule Them All

Browse Source
This commit is contained in:
HFocus
2019-03-17 20:12:30 -06:00
parent 57e4b752d9
commit 10c538d831
10 changed files with 300 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java
+18 -5
View File
@@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.*;
import org.usfirst.frc4388.robot.subsystems.*;
import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode;
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
@@ -74,9 +75,19 @@ public class OI
safteySwitch.whenReleased(new setClimberSafety(false));
safteySwitch.whenReleased(new setLEDPattern(LEDPatterns.FOREST_WAVES));
JoystickButton manual = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
manual.whenPressed(new SetManual());
manual.whenReleased(new SetMMAndPID());
JoystickButton climbUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS);
JoystickButton climbDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_TRIGGER_AXIS);
JoystickButton resetArmEncoder = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.B_BUTTON);
resetArmEncoder.whenPressed(new ResetArmEncoder());
JoystickButton resetWristEncoder = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON);
resetWristEncoder.whenPressed(new ResetWristEncoder());
/** DEPRICATED, TRIGGERS ON THE DRIVER JOYSTICK COVER FOR THIS BUTTON */
//JoystickButton ratchetFlip = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON);
//ratchetFlip.whenPressed(new ratchetFlip(0.5));
@@ -103,13 +114,15 @@ public class OI
JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(),XboxController.LEFT_JOYSTICK_BUTTON);
lowHeight.whenPressed(new GrabFromLoadingSatation());
lowHeight.whenPressed(new SetPositionArmWrist(550, 450));
JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
stow.whenPressed(new StowArm());
stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN));
JoystickButton cargoPlaceMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
cargoPlaceMode.whenPressed(new SwitchArmPlaceMode(PlaceMode.CARGO));
cargoPlaceMode.whenReleased(new SwitchArmPlaceMode(PlaceMode.HATCH));
//stow.whenPressed(new StowArm());
//stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN));
SmartDashboard.putData("switch to manuel", new SetManual());
//SmartDashboard.putData("switch to manuel", new SetManual());
// SmartDashboard.putData("run turn test", new TestTurn());
SmartDashboard.putData("grab from station", new GrabFromLoadingSatation());
SmartDashboard.putData("wrist test", new wristTest());
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+2
View File
@@ -118,6 +118,8 @@ public class Robot extends TimedRobot
//System.err.println("TeleopInit after resetEncoders");
drive.endGyroCalibration();
System.err.println("TeleopInit after endGyroCalibrations");
arm.targetPositionInchesMM = arm.motor1.getPositionWorld();
wrist.targetPositionInchesPID = wrist.wristMotor1.getPositionWorld();
updateStatus();
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetArmEncoder.java
+46
View File
@@ -0,0 +1,46 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
public class ResetArmEncoder extends Command {
public ResetArmEncoder() {
requires(Robot.arm);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
Robot.arm.resetEncoder();
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return true;
}
// Called once after isFinished returns true
@Override
protected void end() {
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetWristEncoder.java
+46
View File
@@ -0,0 +1,46 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
public class ResetWristEncoder extends Command {
public ResetWristEncoder() {
requires(Robot.wrist);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
Robot.wrist.resetencoder();
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return true;
}
// Called once after isFinished returns true
@Override
protected void end() {
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java
+39
View File
@@ -0,0 +1,39 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode;
import edu.wpi.first.wpilibj.command.CommandGroup;
public class SetMMAndPID extends CommandGroup {
/**
* Add your docs here.
*/
public SetMMAndPID() {
addSequential(new ArmSetMode(ArmControlMode.MOTION_MAGIC));
addParallel(new WristSetMode(WristControlMode.JOYSTICK_PID));
// Add Commands here:
// e.g. addSequential(new Command1());
// addSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use addParallel()
// e.g. addParallel(new Command1());
// addSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java
+38
View File
@@ -0,0 +1,38 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package org.usfirst.frc4388.robot.commands;
import edu.wpi.first.wpilibj.command.CommandGroup;
public class SetPositionArmWrist extends CommandGroup {
/**
* Add your docs here.
*/
public SetPositionArmWrist(double arm, double wrist) {
addParallel(new WristSetPositionPID(wrist));
addSequential(new ArmSetPositionMM(arm));
// Add Commands here:
// e.g. addSequential(new Command1());
// addSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use addParallel()
// e.g. addParallel(new Command1());
// addSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java
+3 -2
View File
@@ -17,8 +17,9 @@ public class StowArm extends CommandGroup {
public StowArm() {
addSequential(new HatchFlip(false));
addParallel(new WristPlacement(true));
addParallel(new WristSetPositionPID(200), 2);
addSequential(new ArmSetPositionMM(-10), 4);
addParallel(new WristSetPositionPID(110), 2);
addSequential(new ArmSetPositionMM(420), 4);
addSequential(new ArmSetPositionMM(5));
// Add Commands here:
// e.g. addSequential(new Command1());
// addSequential(new Command2());
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SwitchArmPlaceMode.java
+53
View File
@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode;
import edu.wpi.first.wpilibj.command.Command;
public class SwitchArmPlaceMode extends Command {
public PlaceMode placeMode;
public SwitchArmPlaceMode(PlaceMode placeMode) {
requires(Robot.arm);
this.placeMode = placeMode;
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
Robot.arm.placeMode = placeMode;
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return true;
}
// Called once after isFinished returns true
@Override
protected void end() {
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+49 -13
View File
@@ -4,6 +4,9 @@ import java.util.ArrayList;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.RobotMap;
import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
import org.usfirst.frc4388.robot.commands.SetPositionArmWrist;
import org.usfirst.frc4388.robot.commands.StowArm;
import org.usfirst.frc4388.utility.ControlLoopable;
import org.usfirst.frc4388.utility.Loop;
import org.usfirst.frc4388.utility.MPTalonPIDController;
@@ -33,6 +36,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};
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
public static final double ENCODER_TICKS_TO_INCHES = (1);
@@ -53,8 +57,8 @@ public class Arm extends Subsystem implements ControlLoopable
public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
public static final double ZERO_POSITION_INCHES = -0.25;
public static final double NEAR_ZERO_POSITION_INCHES = 0.0;
public static final double MIN_POSITION_INCHES = -10;
public static final double MAX_POSITION_INCHES = 3900;
public static final double MIN_POSITION_INCHES = -25;
public static final double MAX_POSITION_INCHES = 4400;
public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
public static final double SWITCH_POSITION_INCHES = 24.0;
@@ -75,7 +79,7 @@ public class Arm extends Subsystem implements ControlLoopable
// Motor controllers
private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
private TalonSRXEncoder motor1;
public TalonSRXEncoder motor1;
private TalonSRX motor2;
// PID controller and params
@@ -112,8 +116,9 @@ public class Arm extends Subsystem implements ControlLoopable
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
private boolean isFinished;
private ArmControlMode armControlMode = ArmControlMode.MOTION_MAGIC;
private double targetPositionInchesPID = 0;
private double targetPositionInchesMM = 0;
public PlaceMode placeMode = PlaceMode.HATCH;
public double targetPositionInchesPID = 0;
public double targetPositionInchesMM = 0;
private boolean firstMpPoint;
private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
private double p = 0;
@@ -165,8 +170,11 @@ public class Arm extends Subsystem implements ControlLoopable
public void resetZeroPosition(double position) {
mpController.resetZeroPosition(position);
}
public void resetEncoder(){
motor1.setPosition(0);
targetPositionInchesMM = 0;
targetPositionInchesPID = 0;
}
private synchronized void setArmControlMode(ArmControlMode controlMode) {
@@ -193,9 +201,6 @@ public class Arm extends Subsystem implements ControlLoopable
setArmControlMode(ArmControlMode.MOTION_MAGIC);
updatePositionMM(targetPositionInches);
setFinished(false);
}
public double calcGravityCompensationAtCurrentPosition() {
@@ -255,10 +260,10 @@ public class Arm extends Subsystem implements ControlLoopable
}
private double limitPosition(double targetPosition) {
if (targetPosition < MIN_POSITION_INCHES) {
/*if (targetPosition < MIN_POSITION_INCHES) {
return MIN_POSITION_INCHES;
}
else if (targetPosition > MAX_POSITION_INCHES) {
}*/
if (targetPosition > MAX_POSITION_INCHES) {
return MAX_POSITION_INCHES;
}
@@ -273,9 +278,38 @@ public class Arm extends Subsystem implements ControlLoopable
this.periodMs = periodMs;
}
private int lastDPadAngle = -1;
public void dPadButtons(){
int dPadAngle = Robot.oi.getOperatorController().getDpadAngle();
if (placeMode == PlaceMode.HATCH){
if (dPadAngle == 0 && lastDPadAngle == -1){
new SetPositionArmWrist(3605, 1144).start();
}
if (dPadAngle == 90 && lastDPadAngle == -1){
new SetPositionArmWrist(2000, 750).start();
}
if (dPadAngle == 180 && lastDPadAngle == -1){
new SetPositionArmWrist(590, 450).start();
}
}
if (placeMode == PlaceMode.CARGO) {
if (dPadAngle == 0 && lastDPadAngle == -1){
new SetPositionArmWrist(4298, 3243).start();
}
if (dPadAngle == 90 && lastDPadAngle == -1){
new SetPositionArmWrist(2830, 2830).start();
}
if (dPadAngle == 180 && lastDPadAngle == -1){
new SetPositionArmWrist(1388, 2500).start();
}
}
if (dPadAngle == 270 && lastDPadAngle == -1){
new StowArm().start();
}
SmartDashboard.putNumber("DPad Angle", dPadAngle);
lastDPadAngle = dPadAngle;
}
public synchronized void controlLoopUpdate() {
// Measure *actual* update period
@@ -284,6 +318,8 @@ public class Arm extends Subsystem implements ControlLoopable
lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
}
lastControlLoopUpdateTimestamp = currentTimestamp;
dPadButtons();
if (motor1.getSensorCollection().isRevLimitSwitchClosed()){
resetEncoder();
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+6 -4
View File
@@ -73,7 +73,7 @@ public class Wrist extends Subsystem implements ControlLoopable
// Motor controllers
private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
private TalonSRXEncoder wristMotor1;
public TalonSRXEncoder wristMotor1;
// PID controller and params
private MPTalonPIDController mpController;
@@ -99,7 +99,7 @@ public class Wrist extends Subsystem implements ControlLoopable
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
private boolean isFinished;
private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID;
private double targetPositionInchesPID = 0;
public double targetPositionInchesPID = 0;
private boolean firstMpPoint;
private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
private double p = 0;
@@ -138,6 +138,7 @@ public class Wrist extends Subsystem implements ControlLoopable
}
public void resetencoder(){
wristMotor1.setPosition(0);
targetPositionInchesPID = 0;
}
@@ -192,12 +193,13 @@ public class Wrist extends Subsystem implements ControlLoopable
}
private double limitPosition(double targetPosition) {
if (targetPosition < MIN_POSITION_INCHES) {
/*if (targetPosition < MIN_POSITION_INCHES) {
return MIN_POSITION_INCHES;
}
else if (targetPosition > MAX_POSITION_INCHES) {
return MAX_POSITION_INCHES;
}
}*/
return targetPosition;
}