added more autos that require a gyro
This commit is contained in:
parent
d92680e9ff
commit
4be7508f90
@ -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);
|
||||||
|
@ -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;
|
||||||
|
@ -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();
|
||||||
|
@ -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
|
@ -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
|
@ -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
|
@ -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));
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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));
|
||||||
|
}
|
||||||
|
}
|
@ -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));
|
||||||
|
}
|
||||||
|
}
|
@ -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));
|
||||||
|
}
|
||||||
|
}
|
@ -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());
|
||||||
|
Loading…
Reference in New Issue
Block a user