diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index 5a3cddb..c9c51d8 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.buttons.JoystickButton; import org.usfirst.frc.team2059.robot.commands.LogEncoder; +import org.usfirst.frc.team2059.robot.commands.PIDDrive; import org.usfirst.frc.team2059.robot.commands.MoveArm; import org.usfirst.frc.team2059.robot.commands.SetArmPosition; import org.usfirst.frc.team2059.robot.commands.SetShooterState; @@ -33,6 +34,7 @@ public class OI { //joystickButtons[0][0].whenPressed(new LogEncoder()); joystickButtons[0][0].whileHeld(new SpinRollers(1)); joystickButtons[0][1].whileHeld(new SetShooterState(true)); +// joystickButtons[0][2].whileHeld(new PIDDrive(400)); joystickButtons[1][0].whileHeld(new MoveArm(0.5)); joystickButtons[1][1].whileHeld(new MoveArm(-0.5)); diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index ab15450..e1d1b62 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -16,6 +16,7 @@ public class Robot extends IterativeRobot { chooser = new SendableChooser(); SmartDashboard.putData("Auto mode", chooser); SmartDashboard.putData("MainArm",CommandBase.mainArm.getPIDController()); + SmartDashboard.putData("LeftEncoderController",CommandBase.driveBase.getLeftController()); SmartDashboard.putBoolean("CompressorEnabled",true); //Automatically determine if rolling in or rolling out SmartDashboard.putBoolean("SmartRollers",true); @@ -45,6 +46,7 @@ public class Robot extends IterativeRobot { Scheduler.getInstance().run(); SmartDashboard.putNumber("ArmAngleRaw",CommandBase.mainArm.getRaw()); SmartDashboard.putNumber("ArmAngleDegrees",CommandBase.mainArm.getDegrees()); + SmartDashboard.putNumber("tmpRotations",CommandBase.driveBase.getLeftRotations()); CommandBase.pneumatics.setCompressorEnabled(SmartDashboard.getBoolean("CompressorEnabled")); System.out.println(CommandBase.mainArm.getDegrees()); } diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index 4c82f66..810eb06 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -5,11 +5,11 @@ public class RobotMap { public static int driveLeftMotorTwo = 2; public static int driveRightMotorOne = 3; public static int driveRightMotorTwo = 4; - public static int driveRightEncoder = 0; - public static int driveLeftEncoder = 1; + public static int driveLeftEncoderA = 0; + public static int driveLeftEncoderB = 1; //Arm - public static double zeroDegrees = 2.08; - public static double ninetyDegrees = 3.54; + public static double zeroDegrees = 2.019; + public static double ninetyDegrees = 3.512; public static int armPot = 0; public static int armLeftMotor = 5; public static int armRightMotor = 6; @@ -25,7 +25,7 @@ public class RobotMap { public static int armStopSolenoidOne = 4; public static int armStopSolenoidTwo = 5; //Misc - public static int mainArmPresetCollect = -3; + public static int mainArmPresetCollect = -5; public static int mainArmPresetTraverse = 11; public static int mainArmPresetCloseShot = 90; public static int mainArmPresetFarShot = 70; diff --git a/src/org/usfirst/frc/team2059/robot/commands/PIDDrive.java b/src/org/usfirst/frc/team2059/robot/commands/PIDDrive.java new file mode 100644 index 0000000..b930527 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/PIDDrive.java @@ -0,0 +1,32 @@ +package org.usfirst.frc.team2059.robot.commands; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +/** + * + */ +public class PIDDrive extends CommandBase { + double count; + public PIDDrive(double c) { + requires(driveBase); + count=c; + } + // Called just before this Command runs the first time + protected void initialize() { + } + // Called repeatedly when this Command is scheduled to run + protected void execute() { + driveBase.pidDrive(count); + } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + // Called once after isFinished returns true + protected void end() { + } + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/commands/SetArmPosition.java b/src/org/usfirst/frc/team2059/robot/commands/SetArmPosition.java index 3d347de..c7b7ad3 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/SetArmPosition.java +++ b/src/org/usfirst/frc/team2059/robot/commands/SetArmPosition.java @@ -19,7 +19,7 @@ public class SetArmPosition extends CommandBase { //Move the arm stop if(pos==RobotMap.mainArmPresetCollect){ pneumatics.setArmStopState(true); - }else if((pos!=RobotMap.mainArmPresetCollect)&&(mainArm.getDegrees()>15)){ + }else if((pos!=RobotMap.mainArmPresetCollect)&&(mainArm.getDegrees()>2)){ pneumatics.setArmStopState(false); } mainArm.enable(); diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java index d4a1453..0497449 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java @@ -2,12 +2,19 @@ package org.usfirst.frc.team2059.robot.subsystems; import org.usfirst.frc.team2059.robot.RobotMap; import org.usfirst.frc.team2059.robot.commands.Drive; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.CANTalon; public class DriveBase extends Subsystem { CANTalon leftMotorOne = new CANTalon(RobotMap.driveLeftMotorOne); 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); + + PIDController leftEncoderController = new PIDController(0.02,0.002,0.017,leftEncoder,new MotorsPIDOutput()); + public void initDefaultCommand() { setDefaultCommand(new Drive()); } @@ -17,5 +24,26 @@ public class DriveBase extends Subsystem { rightMotorOne.set(y + (x + z)); rightMotorTwo.set(y + (x + z)); } + public void pidDrive(double setpoint){ + leftEncoder.reset(); + leftEncoderController.enable(); + leftEncoderController.setSetpoint(setpoint); + } + public PIDController getLeftController(){ + return leftEncoderController; + } + public double getLeftRotations(){ + return leftEncoder.get(); + } + + public class MotorsPIDOutput implements PIDOutput{ + @Override + public void pidWrite(double output){ + leftMotorOne.pidWrite(output); + leftMotorTwo.pidWrite(output); + rightMotorOne.pidWrite(-output); + rightMotorTwo.pidWrite(-output); + } + } } // vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/EncoderBase.java b/src/org/usfirst/frc/team2059/robot/subsystems/EncoderBase.java index 0629bb0..8f95286 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/EncoderBase.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/EncoderBase.java @@ -9,7 +9,7 @@ public class EncoderBase extends Subsystem { public EncoderBase() { } //Encoder enc = new Encoder(0, 1, false, Encoder.EncodingType.k1X); - Encoder enc = new Encoder(0, 1); + Encoder enc = new Encoder(8, 9); public void initDefaultCommand() { //TODO: Not sure if we need a default command, not setting one //TODO: These are just defaults, they wil lneed to be changed