added more autos that require a gyro

This commit is contained in:
Adam Long 2016-09-27 01:09:00 +00:00
parent d92680e9ff
commit 4be7508f90
11 changed files with 175 additions and 9 deletions

View File

@ -24,11 +24,13 @@ public class Robot extends IterativeRobot {
chooser.addObject("Time based low bar", new RoutineDriveTime()); chooser.addObject("Time based low bar", new RoutineDriveTime());
chooser.addObject("Time based straight low bar", new RoutineDriveStraightTime()); chooser.addObject("Time based straight low bar", new RoutineDriveStraightTime());
chooser.addObject("Time based defense", new RoutineDefenseTime()); chooser.addObject("Time based defense", new RoutineDefenseTime());
chooser.addObject("Shot Auto", new RoutineShoot());
chooser.addObject("Two Ball Drop off", new RoutineTwoBallDropOff());
SmartDashboard.putData("Auto mode", chooser); SmartDashboard.putData("Auto mode", chooser);
SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController()); SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController());
SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController()); SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController());
SmartDashboard.putData("gyroPID", CommandBase.driveBase.getGyroController()); SmartDashboard.putData("gyroPID", CommandBase.driveBase.getGyroController());
SmartDashboard.putNumber("GyroCorrection",0.187); SmartDashboard.putNumber("GyroCorrection",0.13);
SmartDashboard.putBoolean("CompressorEnabled", true); SmartDashboard.putBoolean("CompressorEnabled", true);
//Automatically determine if rolling in or rolling out //Automatically determine if rolling in or rolling out
SmartDashboard.putBoolean("SmartRollers", false); SmartDashboard.putBoolean("SmartRollers", false);

View File

@ -10,7 +10,7 @@ public class RobotMap {
public static int gyro = 1; public static int gyro = 1;
//Arm //Arm
public static double zeroDegrees = 1.520; public static double zeroDegrees = 1.520;
public static double ninetyDegrees = 3.419; public static double ninetyDegrees = 3.335;
public static int armPot = 0; public static int armPot = 0;
public static int armLeftMotor = 5; public static int armLeftMotor = 5;
public static int armRightMotor = 6; public static int armRightMotor = 6;

View File

@ -3,15 +3,17 @@ import org.usfirst.frc.team2059.robot.commands.CommandBase;
import org.usfirst.frc.team2059.robot.Robot; import org.usfirst.frc.team2059.robot.Robot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class AutoDriveStraightTime extends CommandBase { public class AutoDriveStraightTime extends CommandBase {
public AutoDriveStraightTime(double time) { double speed;
public AutoDriveStraightTime(double speed, double time) {
requires(driveBase); requires(driveBase);
setTimeout(time); setTimeout(time);
this.speed=speed;
} }
protected void initialize() { protected void initialize() {
driveBase.resetGyro(); driveBase.resetGyro();
} }
protected void execute() { protected void execute() {
driveBase.driveStraight(-0.5,SmartDashboard.getNumber("GyroCorrection")); driveBase.driveStraight(-speed,SmartDashboard.getNumber("GyroCorrection"));
} }
protected void end() { protected void end() {
driveBase.stopDriving(); driveBase.stopDriving();

View File

@ -0,0 +1,38 @@
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 AutoMoveArm extends CommandBase {
double speed;
public AutoMoveArm(double s, double timeout) {
requires(mainArm);
speed = s;
setTimeout(timeout);
}
// Called just before this Command runs the first time
protected void initialize() {
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
mainArm.disable();
mainArm.moveArm(speed);
System.out.println(speed);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return isTimedOut();
}
// Called once after isFinished returns true
protected void end() {
mainArm.moveArm(0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
// vim: sw=2:ts=2:sts=2

View File

@ -0,0 +1,32 @@
package org.usfirst.frc.team2059.robot.commands.autonomous;
import org.usfirst.frc.team2059.robot.commands.CommandBase;
import org.usfirst.frc.team2059.robot.Robot;
/**
*
*/
public class AutoSetShooterState extends CommandBase {
boolean state;
public AutoSetShooterState(boolean s) {
state = s;
}
// Called just before this Command runs the first time
protected void initialize() {
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
pneumatics.setShooterState(state);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return true;
}
// 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() {
end();
}
}
// vim: sw=2:ts=2:sts=2

View File

@ -0,0 +1,36 @@
package org.usfirst.frc.team2059.robot.commands.autonomous;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc.team2059.robot.commands.CommandBase;
import org.usfirst.frc.team2059.robot.Robot;
/**
*
*/
public class AutoSpinRollers extends CommandBase {
double speed;
boolean smartrollers;
public AutoSpinRollers(double s, double timeout) {
speed = s;
setTimeout(timeout);
}
// Called just before this Command runs the first time
protected void initialize() {
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
shooter.shootAtSpeed(speed);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return isTimedOut();
}
// Called once after isFinished returns true
protected void end() {
shooter.shootAtSpeed(0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
// vim: sw=2:ts=2:sts=2

View File

@ -6,9 +6,6 @@ public class RoutineDriveStraightTime extends CommandGroup {
public RoutineDriveStraightTime() { public RoutineDriveStraightTime() {
addSequential(new AutoSetArmStopState(true)); addSequential(new AutoSetArmStopState(true));
addSequential(new AutoResetLower(-1)); addSequential(new AutoResetLower(-1));
addSequential(new AutoDriveStraightTime(1)); addSequential(new AutoDriveStraightTime(0.5,1));
addSequential(new AutoRotate(45));
addParallel(new AutoSetArmPosition(90));
addSequential(new AutoDriveStraightTime(1.5));
} }
} }

View File

@ -0,0 +1,13 @@
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 RoutineRaiseArmFire extends CommandGroup {
public RoutineRaiseArmFire() {
addSequential(new AutoSetArmPosition(90));
//Raise the arm a little before spinning rollers
try{ wait(2); }catch(Exception e){ }
addParallel(new AutoSpinRollers(0.5,5));
addSequential(new AutoSetShooterState(true));
}
}

View File

@ -0,0 +1,20 @@
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 RoutineShoot extends CommandGroup {
public RoutineShoot() {
addSequential(new AutoSetArmStopState(true));
addSequential(new AutoResetLower(-1));
addSequential(new AutoDriveStraightTime(0.5,1));
addParallel(new AutoSetArmPosition(90));
addSequential(new AutoRotate(45));
addSequential(new AutoDriveStraightTime(0.4,1.5));
addSequential(new AutoSpinRollers(1,2));
addSequential(new AutoSetShooterState(true));
addSequential(new AutoSetArmStopState(false));
addParallel(new AutoSetArmPosition(10));
addSequential(new AutoRotate(180));
addSequential(new AutoSetShooterState(false));
}
}

View File

@ -0,0 +1,26 @@
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 RoutineTwoBallDropOff extends CommandGroup {
public RoutineTwoBallDropOff() {
addSequential(new AutoSetArmStopState(true));
addSequential(new AutoResetLower(-1));
addSequential(new AutoDriveStraightTime(0.5,2));
addSequential(new AutoMoveArm(0.5,1));
addSequential(new AutoSpinRollers(1,2));
addSequential(new AutoSetShooterState(true));
addSequential(new AutoResetLower(-1));
addSequential(new AutoRotate(180));
addSequential(new AutoSetShooterState(false));
addParallel(new AutoSpinRollers(-0.5,3));
addSequential(new AutoDriveStraightTime(0.5,2));
addSequential(new AutoRotate(180));
addSequential(new AutoDriveStraightTime(0.5,2));
addSequential(new AutoMoveArm(0.5,1));
addSequential(new AutoSpinRollers(1,2));
addSequential(new AutoSetShooterState(true));
addSequential(new AutoRotate(180));
addSequential(new AutoSetShooterState(false));
}
}

View File

@ -17,7 +17,7 @@ public class DriveBase extends Subsystem {
CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorTwo); 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.k2X);
AnalogGyro gyro = new AnalogGyro(RobotMap.gyro); AnalogGyro gyro = new AnalogGyro(RobotMap.gyro);
PIDController gyroController = new PIDController(0.082,0.002,0.07, new GyroPIDSource(), new MotorsPIDOutputReversed()); 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()); PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
public void initDefaultCommand() { public void initDefaultCommand() {
setDefaultCommand(new Drive()); setDefaultCommand(new Drive());