mirror of
https://github.com/Team4388/2018-Robot.git
synced 2026-06-09 00:38:04 -06:00
Auto fixes and new 2 cube autos
This commit is contained in:
Luke Staudacher
@@ -23,7 +23,7 @@ public class Constants {
|
||||
public static double kDriveStraightBasicMinSpeedInchesPerSec = 5.0;
|
||||
public static double kDriveStraightBasicYawErrorDivisor = 20.0; // steer parameter will be -yawError divided by this
|
||||
public static double kDriveStraightBasicMaxSteerMagnitude = 0.8; // keep absolute value of steer parameter below this
|
||||
public static double kDriveTurnBasicTankMotorOutput = 0.5;
|
||||
public static double kDriveTurnBasicTankMotorOutput = 0.4;
|
||||
public static double kDriveTurnBasicSingleMotorOutput = 0.15;
|
||||
public static double kElevatorWheelDiameterInches = 1;
|
||||
// Encoders
|
||||
@@ -35,7 +35,7 @@ public class Constants {
|
||||
public static double kElevatorInchesOfTravelPerRev = 3.75;
|
||||
// public static double kElevatorEncoderTicksPerInch = (double)kElevatorEncoderTickesPerRev / kElevatorInchesOfTravelPerRev;
|
||||
public static double kElevatorEncoderTicksPerInch = 126.36;
|
||||
public static double kElevatorBasicPercentOutputUp = -.8;
|
||||
public static double kElevatorBasicPercentOutputUp = -0.85;
|
||||
public static double kElevatorBasicPercentOutputDown =.7;
|
||||
|
||||
// CONTROL LOOP GAINS
|
||||
|
||||
@@ -192,7 +192,7 @@ public class OI
|
||||
//SmartDashboard.putData("Move Stop", new ElevatorSetSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED));
|
||||
|
||||
|
||||
SmartDashboard.putData("move elevator", new ElevatorBasic(20));
|
||||
//SmartDashboard.putData("move elevator", new ElevatorBasic(20));
|
||||
|
||||
///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED));
|
||||
///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED));
|
||||
@@ -260,7 +260,7 @@ public class OI
|
||||
//Button turnRelativeMP = new InternalButton();
|
||||
//turnRelativeMP.whenPressed(new DriveRelativeTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY ));
|
||||
//SmartDashboard.putData("Turn Relative 10", turnRelativeMP);
|
||||
|
||||
/*
|
||||
Button turnAbsoluteMP = new InternalButton();
|
||||
turnAbsoluteMP.whenPressed(new DriveAbsoluteTurnMP(45, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY));
|
||||
SmartDashboard.putData("Turn Absolute 45", turnAbsoluteMP);
|
||||
@@ -268,13 +268,13 @@ public class OI
|
||||
Button turnRelativePID = new InternalButton();
|
||||
turnRelativePID.whenPressed(new DriveRelativeTurnPID(45, MPSoftwareTurnType.TANK));
|
||||
SmartDashboard.putData("Turn Relative 45", turnRelativePID);
|
||||
|
||||
*/
|
||||
// Button gyroReset = new InternalButton();
|
||||
// gyroReset.whenPressed(new DriveGyroReset());
|
||||
// SmartDashboard.putData("Gyro Reset", gyroReset);
|
||||
SmartDashboard.putData("Reset Gyro", new DriveGyroReset());
|
||||
//SmartDashboard.putData("reset Encoders", new drive.resetEncoders());
|
||||
|
||||
SmartDashboard.putData("clampy boi", new IntakePosition(true));
|
||||
|
||||
// Camera
|
||||
//Button cameraUpdateDashboard = new InternalButton();
|
||||
|
||||
@@ -123,14 +123,13 @@ public class Robot extends IterativeRobot
|
||||
|
||||
RRautonTaskChooser.addDefault("Choose RR Program", new CrossTheBaseLine());
|
||||
|
||||
|
||||
RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
|
||||
|
||||
RRautonTaskChooser.addObject("Right too 2 Cube Scale", new Cube2Right());
|
||||
RRautonTaskChooser.addObject("Center too right 2 Cube", new CenterRight2Cube());
|
||||
RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
|
||||
RRautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
|
||||
//RRautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube());
|
||||
|
||||
RRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
|
||||
RRautonTaskChooser.addObject("Right to Right Scale", new ScaleFrom3());
|
||||
RRautonTaskChooser.addObject("Right to Right tall boi", new ScaleFrom3());
|
||||
|
||||
|
||||
SmartDashboard.putData("RRAuton Task", RRautonTaskChooser);
|
||||
@@ -145,12 +144,13 @@ public class Robot extends IterativeRobot
|
||||
|
||||
|
||||
RLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
|
||||
RLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1());
|
||||
|
||||
RLautonTaskChooser.addObject("Left to Left tall boi", new ScaleFrom1());
|
||||
RLautonTaskChooser.addObject("Center too right 2 Cube", new CenterRight2Cube());
|
||||
RLautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
|
||||
//RLautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube());
|
||||
|
||||
RLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
|
||||
RLautonTaskChooser.addObject("Left to left 2 Cube Scale", new Cube2Left());
|
||||
|
||||
|
||||
|
||||
SmartDashboard.putData("RLAuton Task", RLautonTaskChooser);
|
||||
|
||||
@@ -164,15 +164,12 @@ public class Robot extends IterativeRobot
|
||||
LLautonTaskChooser.addDefault("Choose LL Program", new CrossTheBaseLine());
|
||||
|
||||
|
||||
LLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1());
|
||||
LLautonTaskChooser.addObject("Left to Left tall boi", new ScaleFrom1());
|
||||
LLautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton());
|
||||
|
||||
|
||||
LLautonTaskChooser.addObject("Center too 2 left cube", new CenterLeft2Cube());
|
||||
LLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
|
||||
//LLautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube());
|
||||
|
||||
|
||||
LLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
|
||||
LLautonTaskChooser.addObject("Left to left 2 Cube Scale", new Cube2Left ());
|
||||
|
||||
|
||||
SmartDashboard.putData("LLAuton Task", LLautonTaskChooser);
|
||||
@@ -181,14 +178,15 @@ public class Robot extends IterativeRobot
|
||||
LRautonTaskChooser = new SendableChooser<Command>();
|
||||
|
||||
LRautonTaskChooser.addDefault("Choose LR Program", new CrossTheBaseLine());
|
||||
|
||||
|
||||
LRautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton());
|
||||
|
||||
LRautonTaskChooser.addObject("Right to 2 Cube Scale", new Cube2Right());
|
||||
LRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
|
||||
//LRautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube());
|
||||
|
||||
LRautonTaskChooser.addObject("Center too left 2 cube", new CenterLeft2Cube());
|
||||
LRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
|
||||
LRautonTaskChooser.addObject("Right To Right Scale", new ScaleFrom3());
|
||||
LRautonTaskChooser.addObject("Right To Right tall boi", new ScaleFrom3());
|
||||
|
||||
|
||||
SmartDashboard.putData("LRAuton Task", LRautonTaskChooser);
|
||||
|
||||
|
||||
@@ -37,7 +37,7 @@ public class CenterLeft extends CommandGroup {
|
||||
addSequential(new DriveTurnBasic(true, 28, 150, MPSoftwareTurnType.TANK));
|
||||
addParallel(new TimeoutBecaseYea());
|
||||
addSequential(new DriveStraightBasic(12, 45, true, true, 0));
|
||||
//addParallel(new TimeoutBecaseYea());
|
||||
|
||||
addSequential(new WaitCommand(3));
|
||||
|
||||
|
||||
|
||||
@@ -33,30 +33,26 @@ public class CenterLeft2Cube extends CommandGroup {
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new DriveStraightBasic(80, 45, true, true, 0));
|
||||
addSequential(new ElevatorBasic(30));
|
||||
addSequential(new DriveTurnBasic(true, 17, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasic(10, 45, true, true, 0));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addSequential(new DriveTurnBasic(true, 28, 150, MPSoftwareTurnType.TANK));
|
||||
addParallel(new TimeoutBecaseYea());
|
||||
addSequential(new DriveStraightBasic(12, 45, true, true, 0));
|
||||
addSequential(new WaitCommand(1));
|
||||
addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
|
||||
addSequential(new ElevatorBasic(10));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new ElevatorBasic(2));
|
||||
addSequential(new DriveTurnBasic(true, -90, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new ElevatorBasic(3));
|
||||
addSequential(new DriveTurnBasic(true, 75, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
|
||||
addSequential(new DriveStraightBasic(20, 45, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(45, 45, true, true, 0));
|
||||
addSequential(new WaitCommand(2));
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
|
||||
addSequential(new ElevatorBasic(25));
|
||||
addSequential(new DriveTurnBasic(true, 90, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasic(10, 45, true, true, 0));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new DriveStraightBasic(-40, 45, true, true, 0));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addSequential(new ElevatorBasic(25));
|
||||
addSequential(new DriveTurnBasic(true, -75, 150, MPSoftwareTurnType.TANK));
|
||||
addParallel(new TimeoutBecaseYea());
|
||||
addSequential(new DriveStraightBasic(20, 45, true, true, 0));
|
||||
|
||||
addSequential(new WaitCommand(1));
|
||||
|
||||
addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
|
||||
|
||||
addSequential(new DriveSpeedShift(false));
|
||||
|
||||
@@ -1,46 +0,0 @@
|
||||
package org.usfirst.frc4388.robot.commands.auton;
|
||||
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.DriveGyroReset;
|
||||
import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightBasicAbs;
|
||||
import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
|
||||
import org.usfirst.frc4388.robot.commands.ElevatorBasic;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.IntakePosition;
|
||||
import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
|
||||
import org.usfirst.frc4388.robot.subsystems.Carriage;
|
||||
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import edu.wpi.first.wpilibj.command.WaitCommand;
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class CenterLeftAbs extends CommandGroup {
|
||||
|
||||
public CenterLeftAbs()
|
||||
{
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
|
||||
addSequential(new DriveStraightBasicAbs(-15, 50, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasicAbs(53, 50, true, true, 130));
|
||||
addSequential(new ElevatorBasic(20));
|
||||
addSequential(new DriveTurnBasic(true, 164.5, 300, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasicAbs(19, 50, true, true, 164.5));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addSequential(new DriveStraightBasicAbs(-20, 50, true, true, 164.5));
|
||||
addSequential(new ElevatorBasic(10));
|
||||
addSequential(new DriveSpeedShift(false));
|
||||
}
|
||||
}
|
||||
@@ -18,40 +18,35 @@ public class CenterRight2Cube extends CommandGroup {
|
||||
public CenterRight2Cube()
|
||||
{
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
|
||||
addSequential(new DriveStraightBasic(-15, 30, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(-15, 60, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, -115, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new DriveStraightBasic(80, 45, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(65, 60, true, true, 0));
|
||||
addSequential(new ElevatorBasic(25));
|
||||
addSequential(new DriveTurnBasic(true, -35, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasic(15, 45, true, true, 0));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addSequential(new DriveStraightBasic(-25, 45, true, true, 0));
|
||||
addSequential(new ElevatorBasic(10));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new ElevatorBasic(2));
|
||||
addSequential(new DriveTurnBasic(true, 90, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
|
||||
addSequential(new DriveStraightBasic(20, 45, true, true, 0));
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addSequential(new DriveTurnBasic(true, -38, 150, MPSoftwareTurnType.TANK));
|
||||
addParallel(new TimeoutBecaseYea());
|
||||
addSequential(new DriveStraightBasic(25, 45, true, true, 0));
|
||||
addSequential(new WaitCommand(1));
|
||||
addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
|
||||
|
||||
addSequential(new ElevatorBasic(25));
|
||||
addSequential(new DriveTurnBasic(true, -90, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasic(10, 45, true, true, 0));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new ElevatorBasic(3));
|
||||
addSequential(new DriveTurnBasic(true, -75, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
|
||||
addSequential(new DriveStraightBasic(45, 45, true, true, 0));
|
||||
addSequential(new WaitCommand(2));
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new DriveStraightBasic(-40, 60, true, true, 0));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addSequential(new ElevatorBasic(25));
|
||||
addSequential(new DriveTurnBasic(true, 75, 150, MPSoftwareTurnType.TANK));
|
||||
addParallel(new TimeoutBecaseYea());
|
||||
addSequential(new DriveStraightBasic(20, 45, true, true, 0));
|
||||
|
||||
addSequential(new WaitCommand(1));
|
||||
|
||||
addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
|
||||
|
||||
addSequential(new DriveSpeedShift(false));
|
||||
|
||||
@@ -1,45 +0,0 @@
|
||||
package org.usfirst.frc4388.robot.commands.auton;
|
||||
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.DriveGyroReset;
|
||||
import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightBasicAbs;
|
||||
import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
|
||||
import org.usfirst.frc4388.robot.commands.ElevatorBasic;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.IntakePosition;
|
||||
import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
|
||||
import org.usfirst.frc4388.robot.subsystems.Carriage;
|
||||
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import edu.wpi.first.wpilibj.command.WaitCommand;
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class CenterRightAbs extends CommandGroup {
|
||||
|
||||
public CenterRightAbs()
|
||||
{
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
|
||||
addSequential(new DriveStraightBasicAbs(-15, 50, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, -130, 300, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasicAbs(53, 50, true, true, -130));
|
||||
addSequential(new ElevatorBasic(20));
|
||||
addSequential(new DriveTurnBasic(true, -164.5, 300, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasicAbs(19, 50, true, true, -164.5));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addSequential(new DriveStraightBasic(-20, 50, true, true, -164.5));
|
||||
addSequential(new DriveSpeedShift(false));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
package org.usfirst.frc4388.robot.commands.auton;
|
||||
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.DriveGyroReset;
|
||||
import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
|
||||
import org.usfirst.frc4388.robot.commands.ElevatorBasic;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.IntakePosition;
|
||||
import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
|
||||
import org.usfirst.frc4388.robot.subsystems.Carriage;
|
||||
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import edu.wpi.first.wpilibj.command.WaitCommand;
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class Cube2Left extends CommandGroup {
|
||||
|
||||
public Cube2Left() {
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
addSequential(new IntakePosition(true));
|
||||
|
||||
addSequential(new DriveStraightBasic(-300, 70, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(1, 30, true, true, 0));
|
||||
addSequential(new ElevatorBasic(70));
|
||||
addSequential(new DriveTurnBasic(true, -90, 100, MPSoftwareTurnType.TANK));
|
||||
addSequential(new WaitCommand(.5));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.5));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
|
||||
addSequential(new DriveStraightBasic(-10, 30, true, true, 0));
|
||||
addParallel(new ElevatorBasic(4));
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
addSequential(new DriveTurnBasic(true, 69, 100, MPSoftwareTurnType.TANK));
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_FAST_SPEED));
|
||||
|
||||
/////////////////////////////////////////////////////////////
|
||||
addSequential(new DriveStraightBasic(10, 30, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(80, 60, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(33, 20, true, true, 0));
|
||||
/////////////////////////////////////////////////////////////
|
||||
addSequential(new WaitCommand(.5));
|
||||
addSequential(new IntakePosition(true));
|
||||
//addSequential(new WaitCommand(.3));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addParallel(new TimeoutBecaseYea());
|
||||
addSequential(new ElevatorBasic(28));
|
||||
/////////////////////////////////////////////////////////////
|
||||
addSequential(new DriveStraightBasic(6, 50, true, true, 0));
|
||||
/////////////////////////////////////////////////////////////
|
||||
addSequential(new WaitCommand(1.5));
|
||||
addSequential(new DriveStraightBasic(-20, 40, true, true, 0));
|
||||
addSequential(new ElevatorBasic(5));
|
||||
|
||||
|
||||
addSequential(new DriveSpeedShift(false));
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -24,40 +24,52 @@ public class Cube2Right extends CommandGroup {
|
||||
|
||||
public Cube2Right() {
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
addSequential(new IntakePosition(true));
|
||||
|
||||
|
||||
addSequential(new DriveStraightBasic(-140, 50, true, true, 0));
|
||||
addSequential(new ElevatorBasic(30));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new ElevatorBasic(30));
|
||||
addSequential(new DriveStraightBasic(8, 50, true, true, 0));
|
||||
//addSequential(new DriveStraightBasic(-10, 35, true, true, 0));
|
||||
|
||||
addSequential(new DriveStraightBasic(-300, 70, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(1, 30, true, true, 0));
|
||||
addSequential(new ElevatorBasic(70));
|
||||
addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK));
|
||||
addSequential(new WaitCommand(.5));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.4));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addSequential(new DriveStraightBasic(-10, 50, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, 40, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasic(-30, 50, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, -10, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
|
||||
addSequential(new DriveStraightBasic(30, 50, true, true, 0));
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addSequential(new DriveStraightBasic(-30, 50, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, 30, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new ElevatorBasic(70));
|
||||
addSequential(new DriveStraightBasic(-20, 20, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.5));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
|
||||
addSequential(new DriveStraightBasic(-10, 30, true, true, 0));
|
||||
addParallel(new ElevatorBasic(4));
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
addSequential(new DriveTurnBasic(true, -69, 100, MPSoftwareTurnType.TANK));
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_FAST_SPEED));
|
||||
|
||||
/////////////////////////////////////////////////////////////
|
||||
addSequential(new DriveStraightBasic(10, 30, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(80, 60, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(33, 20, true, true, 0));
|
||||
/////////////////////////////////////////////////////////////
|
||||
addSequential(new WaitCommand(.5));
|
||||
addSequential(new IntakePosition(true));
|
||||
//addSequential(new WaitCommand(.3));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
addParallel(new TimeoutBecaseYea());
|
||||
addSequential(new ElevatorBasic(28));
|
||||
/////////////////////////////////////////////////////////////
|
||||
addSequential(new DriveStraightBasic(6, 50, true, true, 0));
|
||||
/////////////////////////////////////////////////////////////
|
||||
addSequential(new WaitCommand(1.5));
|
||||
addSequential(new DriveStraightBasic(-20, 40, true, true, 0));
|
||||
addSequential(new ElevatorBasic(5));
|
||||
|
||||
|
||||
|
||||
addSequential(new DriveSpeedShift(false));
|
||||
|
||||
@@ -30,7 +30,7 @@ public class LeftStartRightScore extends CommandGroup {
|
||||
addSequential(new IntakePosition(true));
|
||||
|
||||
|
||||
addSequential(new DriveStraightBasic(-215, 40, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(-215, 75, true, true, 0));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new ElevatorBasic(5));
|
||||
addSequential(new DriveTurnBasic(true, -73, 150, MPSoftwareTurnType.TANK));
|
||||
|
||||
@@ -30,7 +30,7 @@ public class RightStartLeftScore extends CommandGroup {
|
||||
addSequential(new IntakePosition(true));
|
||||
|
||||
|
||||
addSequential(new DriveStraightBasic(-215, 40, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(-215, 60, true, true, 0));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new ElevatorBasic(5));
|
||||
addSequential(new DriveTurnBasic(true, 73, 150, MPSoftwareTurnType.TANK));
|
||||
|
||||
@@ -29,7 +29,7 @@ public class ScaleFrom3 extends CommandGroup {
|
||||
addSequential(new IntakePosition(true));
|
||||
|
||||
|
||||
addSequential(new DriveStraightBasic(-10, 45, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(-10, 20, true, true, 0));
|
||||
|
||||
addSequential(new DriveStraightBasic(-270, 45, true, true, 0));
|
||||
addSequential(new ElevatorBasic(70));
|
||||
|
||||
@@ -24,7 +24,7 @@ public class TimeoutBecaseYea extends CommandGroup {
|
||||
|
||||
public TimeoutBecaseYea()
|
||||
{
|
||||
addSequential(new WaitCommand(2));
|
||||
addSequential(new WaitCommand(1.5));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
|
||||
@@ -35,7 +35,8 @@ public class Carriage extends Subsystem {
|
||||
private WPI_TalonSRX carriageRight;
|
||||
public static enum CarriageControlMode { JOYSTICK, MP_STRAIGHT, HOLD, MANUAL};
|
||||
public static final double CUBE_INTAKE_SPEED = 0.40;
|
||||
public static final double CUBE_EJECT_SPEED = -0.8;
|
||||
public static final double CUBE_INTAKE_FAST_SPEED = 0.50;
|
||||
public static final double CUBE_EJECT_SPEED = -0.75;
|
||||
public static final double CUBE_STOP_SPEED = 0;
|
||||
/////^^^^^^^^^ replace this line with the modes we need
|
||||
|
||||
|
||||
Reference in New Issue
Block a user