diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index 45ff3ce..71f9942 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -46,9 +46,6 @@ public class OI { joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse)); joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot)); joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetLowShot)); - joystickButtons[1][7].whileHeld(new SetArmStopState(true)); -// joystickButtons[1][10].whileHeld(new ResetLower(-1)); -// joystickButtons[1][11].whileHeld(new ResetUpper(1)); } public Joystick[] getJoysticks() { return joysticks; diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index aa1019e..feafbab 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -22,10 +22,12 @@ public class Robot extends IterativeRobot { cameraServer.startAutomaticCapture("cam0"); chooser.addDefault("Nothing", new RoutineNothing()); chooser.addObject("Time based low bar", new RoutineDriveTime()); + chooser.addObject("Time based straight low bar", new RoutineDriveStraightTime()); chooser.addObject("Time based defense", new RoutineDefenseTime()); SmartDashboard.putData("Auto mode", chooser); SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController()); SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController()); + SmartDashboard.putNumber("GyroCorrection",0.0); SmartDashboard.putBoolean("CompressorEnabled", true); //Automatically determine if rolling in or rolling out SmartDashboard.putBoolean("SmartRollers", false); diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index 4a95871..79ff052 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -7,9 +7,10 @@ public class RobotMap { public static int driveRightMotorTwo = 4; public static int driveLeftEncoderA = 0; public static int driveLeftEncoderB = 1; + public static int gyro = 1; //Arm - public static double zeroDegrees = 1.622; - public static double ninetyDegrees = 3.234; + public static double zeroDegrees = 1.520; + public static double ninetyDegrees = 3.419; public static int armPot = 0; public static int armLeftMotor = 5; public static int armRightMotor = 6; diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveStraightTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveStraightTime.java new file mode 100644 index 0000000..63281c5 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveStraightTime.java @@ -0,0 +1,26 @@ +package org.usfirst.frc.team2059.robot.commands.autonomous; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +public class AutoDriveStraightTime extends CommandBase { + public AutoDriveStraightTime(double time) { + requires(driveBase); + setTimeout(time); + } + protected void initialize() { + driveBase.resetGyro(); + } + protected void execute() { + driveBase.driveStraight(-0.5,SmartDashboard.getNumber("GyroCorrection")); + } + protected void end() { + driveBase.stopDriving(); + } + protected void interrupted() { + end(); + } + protected boolean isFinished() { + return isTimedOut(); + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java index 720a6e8..1b7dae9 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java @@ -6,6 +6,6 @@ public class RoutineDefenseTime extends CommandGroup { public RoutineDefenseTime() { addSequential(new AutoSetArmStopState(false)); addSequential(new AutoSetArmPosition(6)); - addSequential(new AutoDriveTime(1.5, 0.8)); + addSequential(new AutoDriveTime(2.5, 0.8)); } } diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveStraightTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveStraightTime.java new file mode 100644 index 0000000..9475dc9 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveStraightTime.java @@ -0,0 +1,11 @@ +package org.usfirst.frc.team2059.robot.commands.autonomous; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import edu.wpi.first.wpilibj.command.CommandGroup; +import org.usfirst.frc.team2059.robot.Robot; +public class RoutineDriveStraightTime extends CommandGroup { + public RoutineDriveStraightTime() { + addSequential(new AutoSetArmStopState(true)); + addSequential(new AutoResetLower(-1)); + addSequential(new AutoDriveStraightTime(2)); + } +} diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java index 689a421..ffc416c 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java @@ -6,6 +6,6 @@ public class RoutineDriveTime extends CommandGroup { public RoutineDriveTime() { addSequential(new AutoSetArmStopState(true)); addSequential(new AutoResetLower(-1)); - addSequential(new AutoDriveTime(1.5, 0.75)); + addSequential(new AutoDriveTime(2.5, 0.75)); } } diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java index c670bc9..df6649b 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team2059.robot.subsystems; import org.usfirst.frc.team2059.robot.RobotMap; import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; @@ -14,7 +15,7 @@ public class DriveBase extends Subsystem { CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorTwo); Encoder leftEncoder = new Encoder(RobotMap.driveLeftEncoderA, RobotMap.driveLeftEncoderB, false, Encoder.EncodingType.k2X); AnalogGyro gyro = new AnalogGyro(RobotMap.gyro); - + PIDController gyroController = new PIDController(0.0,0.0,0.0, gyro, new MotorsPIDOutput()); PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput()); public void initDefaultCommand() { setDefaultCommand(new Drive()); @@ -32,6 +33,7 @@ public class DriveBase extends Subsystem { rightMotorTwo.set((y + (x + z)) * sensitivity); } public void driveStraight(double y, double correction){ + SmartDashboard.putNumber("GyroAngle",gyro.getAngle()); leftMotorOne.set((-y + gyro.getAngle()*correction)); leftMotorTwo.set((-y + gyro.getAngle()*correction)); rightMotorOne.set((y + gyro.getAngle()*correction)); @@ -48,6 +50,9 @@ public class DriveBase extends Subsystem { public double getLeftRotations() { return leftEncoder.get(); } + public void resetGyro(){ + gyro.reset(); + } public class MotorsPIDOutput implements PIDOutput { @Override public void pidWrite(double output) {