diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index 00acc4e..8244317 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -23,6 +23,7 @@ public class Robot extends IterativeRobot { chooser.addDefault("Nothing", new RoutineNothing()); chooser.addObject("Time based low bar", new RoutineDriveTime()); chooser.addObject("Time based straight low bar", new RoutineDriveStraightTime()); + chooser.addObject("Distance based low bar straight", new RoutineDriveStraightDistance()); chooser.addObject("Time based defense", new RoutineDefenseTime()); chooser.addObject("Shot Auto", new RoutineShoot()); chooser.addObject("Two Ball Drop off", new RoutineTwoBallDropOff()); diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index 0330459..cc11afd 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -9,8 +9,8 @@ public class RobotMap { public static int driveLeftEncoderB = 1; public static int gyro = 1; //Arm - public static double zeroDegrees = 1.520; - public static double ninetyDegrees = 3.335; + public static double zeroDegrees = 1.469; + public static double ninetyDegrees = 3.274; 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/AutoDrive.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDrive.java index 5e0d63f..708abcb 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDrive.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDrive.java @@ -1,6 +1,7 @@ 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 AutoDrive extends CommandBase { private double distance; // Determines if we should start driving @@ -21,7 +22,7 @@ public class AutoDrive extends CommandBase { } protected void execute() { if (startDriving) { - driveBase.pidDrive(distance); + driveBase.pidDrive(distance,SmartDashboard.getNumber("GyroCorrection")); } startDriving = false; } diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveStraightDistance.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveStraightDistance.java new file mode 100644 index 0000000..71dfc33 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveStraightDistance.java @@ -0,0 +1,30 @@ +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 AutoDriveStraightDistance extends CommandBase { + double distance; + public AutoDriveStraightDistance(double distance, double timeout) { + requires(driveBase); + setTimeout(timeout); + this.distance=distance; + } + protected void initialize() { + driveBase.resetGyro(); + driveBase.getLeftEncoder().reset(); + } + protected void execute() { + driveBase.pidDrive(distance,SmartDashboard.getNumber("GyroCorrection")); + } + protected void end() { + driveBase.getLeftController().disable(); + driveBase.stopDriving(); + } + protected void interrupted() { + end(); + } + protected boolean isFinished() { + return isTimedOut() || Math.abs(distance-driveBase.getLeftRotations()) <= 3; + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveStraightDistance.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveStraightDistance.java new file mode 100644 index 0000000..f61f480 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveStraightDistance.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 RoutineDriveStraightDistance extends CommandGroup { + public RoutineDriveStraightDistance() { + addSequential(new AutoSetArmStopState(true)); + addSequential(new AutoResetLower(-1)); + addSequential(new AutoDriveStraightDistance(200,5)); + } +} diff --git a/src/org/usfirst/frc/team2059/robot/commands/drivetrain/PIDDrive.java b/src/org/usfirst/frc/team2059/robot/commands/drivetrain/PIDDrive.java index 606bd03..4340c42 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/drivetrain/PIDDrive.java +++ b/src/org/usfirst/frc/team2059/robot/commands/drivetrain/PIDDrive.java @@ -1,5 +1,6 @@ package org.usfirst.frc.team2059.robot.commands.drivetrain; import org.usfirst.frc.team2059.robot.commands.CommandBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc.team2059.robot.Robot; /** * @@ -15,7 +16,7 @@ public class PIDDrive extends CommandBase { } // Called repeatedly when this Command is scheduled to run protected void execute() { - driveBase.pidDrive(count); + driveBase.pidDrive(count,SmartDashboard.getNumber("GyroCorrection")); } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java index 3029439..15db764 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java @@ -21,7 +21,7 @@ public class ResetLower extends CommandBase { // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { // Stop when either limit switch is hit - return mainArm.getBottomPressed() || mainArm.getTopPressed(); + return mainArm.getBottomPressed(); } // Called once after isFinished returns true protected void end() { diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java index d7e425a..3fd6907 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java @@ -15,10 +15,11 @@ public class DriveBase extends Subsystem { CANTalon leftMotorTwo = new CANTalon(RobotMap.driveLeftMotorTwo); CANTalon rightMotorOne = new CANTalon(RobotMap.driveRightMotorOne); CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorTwo); - Encoder leftEncoder = new Encoder(RobotMap.driveLeftEncoderA, RobotMap.driveLeftEncoderB, false, Encoder.EncodingType.k2X); + Encoder leftEncoder = new Encoder(RobotMap.driveLeftEncoderA, RobotMap.driveLeftEncoderB, false, Encoder.EncodingType.k4X); AnalogGyro gyro = new AnalogGyro(RobotMap.gyro); PIDController gyroController = new PIDController(0.07,0.002,0.08, new GyroPIDSource(), new MotorsPIDOutputReversed()); PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput()); + double encoderGyroCorrection=1; public void initDefaultCommand() { setDefaultCommand(new Drive()); } @@ -41,10 +42,11 @@ public class DriveBase extends Subsystem { rightMotorOne.set((y + -gyro.getAngle()*correction)); rightMotorTwo.set((y + -gyro.getAngle()*correction)); } - public void pidDrive(double setpoint) { + public void pidDrive(double setpoint, double correction) { leftEncoder.reset(); leftEncoderController.enable(); leftEncoderController.setSetpoint(setpoint); + encoderGyroCorrection=-gyro.getAngle()*correction; } public void rotateAngle(double angle){ SmartDashboard.putNumber("GyroAngle",gyro.getAngle()); @@ -63,16 +65,19 @@ public class DriveBase extends Subsystem { public void resetGyro(){ gyro.reset(); } + public Encoder getLeftEncoder(){ + return leftEncoder; + } public AnalogGyro getGyro(){ return gyro; } public class MotorsPIDOutput implements PIDOutput { @Override public void pidWrite(double output) { - leftMotorOne.pidWrite(output); - leftMotorTwo.pidWrite(output); - rightMotorOne.pidWrite(-output); - rightMotorTwo.pidWrite(-output); + leftMotorOne.pidWrite(output + -encoderGyroCorrection); + leftMotorTwo.pidWrite(output + -encoderGyroCorrection); + rightMotorOne.pidWrite(-output + -encoderGyroCorrection); + rightMotorTwo.pidWrite(-output + -encoderGyroCorrection); } } public class MotorsPIDOutputReversed implements PIDOutput { diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index fb9ff54..d1ae4eb 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -79,11 +79,12 @@ public class MainArm extends PIDSubsystem { System.out.println("Calibrating bottom to: " + getRaw()); min = getRaw(); return true; - } else if (getTopPressed()) { - System.out.println("Calibrating top to: " + getRaw()); - max = getRaw(); - return true; } +// } else if (getTopPressed()) { +// System.out.println("Calibrating top to: " + getRaw()); +// max = getRaw(); +// return true; +// } return false; } }