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 f162457..65d5e7a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -105,7 +105,7 @@ public class OI stow.whenPressed(new StowArm()); SmartDashboard.putData("switch to manuel", new SetManual()); - SmartDashboard.putData("run arm test", new ArmTest()); +// SmartDashboard.putData("run turn test", new TestTurn()); SmartDashboard.putData("grab from station", new GrabFromLoadingSatation()); SmartDashboard.putData("wrist test", new wristTest()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java index a74f364..71dbb4d 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java @@ -69,10 +69,10 @@ public class DriveTurnBasic extends Command } else { Robot.drive.rawDriveLeftRight(output, output); // left forward, right backward } -// for (CANTalonEncoder motorController : motorControllers) { -// //motorController.set(output); -// motorController.set(ControlMode.PercentOutput, output); -// } + //for (CANTalonEncoder motorController : motorControllers) { + //motorController.set(output); + // motorController.set(ControlMode.PercentOutput, output); + //} } else if (m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { Robot.drive.rawDriveLeftRight(2.0 * output, 0.0); // left forward double speed diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java index ed3813a..239bee8 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java @@ -16,8 +16,8 @@ public class StowArm extends CommandGroup { public StowArm() { addSequential(new HatchFlip(false)); addParallel(new WristPlacement(true)); - addParallel(new WristSetPositionPID(200), 2); - addSequential(new ArmSetPositionMM(50)); + addParallel(new WristSetPositionPID(250), 2); + addSequential(new ArmSetPositionMM(10), 4); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/TestTurn.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/TestTurn.java new file mode 100644 index 0000000..a36fe40 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/TestTurn.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class TestTurn extends CommandGroup { + /** + * Add your docs here. + */ + public TestTurn() { + addSequential(new DriveTurnBasic (true, 90, 100, MPSoftwareTurnType.TANK)); + // 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. + } +} 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 00b9501..07db110 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 @@ -111,7 +111,7 @@ public class Arm extends Subsystem implements ControlLoopable // Misc public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; - private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; + private ArmControlMode armControlMode = ArmControlMode.MOTION_MAGIC; private double targetPositionInchesPID = 0; private double targetPositionInchesMM = 0; private boolean firstMpPoint; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index f10b70c..f287c6f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -61,7 +61,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; */ public class Drive extends Subsystem implements ControlLoopable { - public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW}; + public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, STOP_MOTORS, RAW}; public static enum SpeedShiftState { HI, LO }; public static enum ClimberState { DEPLOYED, RETRACTED }; 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 7114f97..855f456 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 @@ -98,7 +98,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.JOYSTICK_MANUAL; + private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID; private double targetPositionInchesPID = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;