From a02dc3dfd738e566ebdd7ea311c4276e5e9f8e1c Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Mon, 18 Mar 2019 15:47:48 -0600 Subject: [PATCH] added changes from 3/18/19 changes for drivers. --- .../java/org/usfirst/frc4388/robot/OI.java | 9 ++- .../frc4388/robot/commands/ArmSetMode.java | 5 +- .../frc4388/robot/commands/HatchFlip.java | 1 + .../frc4388/robot/commands/togglemanuel.java | 68 +++++++++++++++++++ .../usfirst/frc4388/robot/subsystems/Arm.java | 9 +-- .../frc4388/robot/subsystems/Wrist.java | 4 +- 6 files changed, 87 insertions(+), 9 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/togglemanuel.java 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 b08f4fb..a23da6c 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -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.ArmControlMode; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; @@ -79,7 +80,7 @@ public class OI /** 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)); + //ratchetFlip.toggleWhenPressed(new ratchetFlip(0.5)); //ratchetFlip.whenReleased(new ratchetFlip(0)); JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); @@ -89,7 +90,10 @@ public class OI JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); shiftDown.whenPressed(new DriveSpeedShift(false)); // shiftDown.whenPressed(new LEDIndicators(false)); - //Operator Xbox + //Operator Xbox + JoystickButton help = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.X_BUTTON); + help.whenPressed(new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL)); + help.whenReleased(new ArmSetMode(ArmControlMode.MOTION_MAGIC)); /* JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); openIntake.whenPressed(new IntakePosition(true)); @@ -113,6 +117,7 @@ public class OI // SmartDashboard.putData("run turn test", new TestTurn()); SmartDashboard.putData("grab from station", new GrabFromLoadingSatation()); SmartDashboard.putData("wrist test", new wristTest()); + //SmartDashboard.putData("PRE GAME!!!!", new PreGame()); } catch (Exception e) { diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java index 25ccaf4..3eb2046 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java @@ -24,7 +24,10 @@ public class ArmSetMode extends Command { } else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) { Robot.arm.setSpeedJoystick(0); - } + } + else if (controlMode == ArmControlMode.MOTION_MAGIC){ + Robot.arm.setPositionMM(Robot.arm.getPositionInches()); + } else { Robot.arm.setSpeed(0.0); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/HatchFlip.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/HatchFlip.java index 4dfe82e..9466ede 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/HatchFlip.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/HatchFlip.java @@ -16,6 +16,7 @@ public class HatchFlip extends Command @Override protected void initialize() { Robot.pnumatics.setHatchIntakeState(PickingUp); + } @Override diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/togglemanuel.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/togglemanuel.java new file mode 100644 index 0000000..a86b1f5 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/togglemanuel.java @@ -0,0 +1,68 @@ +/*----------------------------------------------------------------------------*/ +/* 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; +import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; + +import edu.wpi.first.wpilibj.command.Command; + + +public class togglemanuel extends Command { + public togglemanuel() { + System.err.print("togle"); + if (Robot.arm.getArmControlMode() == ArmControlMode.MOTION_MAGIC){ + new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL); + System.err.print("manuel"); + } + else if (Robot.arm.getArmControlMode() == ArmControlMode.JOYSTICK_MANUAL){ + new ArmSetMode(ArmControlMode.MOTION_MAGIC); + System.err.print("Smart control"); + } + else{ + new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL); + } + + + } + + + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.err.print("pls work"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // 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() { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 44733a9..d550938 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -173,7 +173,7 @@ public class Arm extends Subsystem implements ControlLoopable this.armControlMode = controlMode; } - private synchronized ArmControlMode getArmControlMode() { + public synchronized ArmControlMode getArmControlMode() { return this.armControlMode; } @@ -188,7 +188,7 @@ public class Arm extends Subsystem implements ControlLoopable } public void setPositionMM(double targetPositionInches){ motor1.set(ControlMode.MotionMagic, targetPositionInches); - System.err.println(motor1.getControlMode()); + //System.err.println(motor1.getControlMode()); motor1.selectProfileSlot(MM_SLOT, 0); setArmControlMode(ArmControlMode.MOTION_MAGIC); updatePositionMM(targetPositionInches); @@ -202,7 +202,7 @@ public class Arm extends Subsystem implements ControlLoopable int ticks = motor1.getSelectedSensorPosition(); double degreesFromDown = (ticks+920)*(360.0/(4096*3)); double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180); - System.err.println("comp(" + degreesFromDown + "^) = " + compensation); + //System.err.println("comp(" + degreesFromDown + "^) = " + compensation); return compensation; } public void updatePositionMM(double targetPositionInches){ @@ -292,6 +292,7 @@ public class Arm extends Subsystem implements ControlLoopable // Do the update if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); + System.err.println(motor1.getControlMode()); } else if (!isFinished) { if (armControlMode == ArmControlMode.MOTION_PROFILE) { @@ -304,7 +305,7 @@ public class Arm extends Subsystem implements ControlLoopable } if (armControlMode == ArmControlMode.MOTION_MAGIC){ controlMMWithJoystick(); - //System.err.println(motor1.getControlMode()); + System.err.println(motor1.getControlMode()); } /*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) { 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 07ccd34..d5353c0 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 @@ -52,7 +52,7 @@ public class Wrist extends Subsystem implements ControlLoopable 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 = 0.0; - public static final double MAX_POSITION_INCHES = 4096; + public static final double MAX_POSITION_INCHES = 3350; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; public static final double SWITCH_POSITION_INCHES = 24.0; @@ -278,7 +278,7 @@ public class Wrist extends Subsystem implements ControlLoopable } if (wristControlMode == WristControlMode.JOYSTICK_PID){ - System.err.println(wristMotor1.getControlMode()); + //System.err.println(wristMotor1.getControlMode()); controlPidWithJoystick();