changed pid values back, added blank auto, changed drive times in auto
This commit is contained in:
parent
4e86a8e6fa
commit
e0c78e71b4
@ -20,7 +20,8 @@ public class Robot extends IterativeRobot {
|
||||
cameraServer = CameraServer.getInstance();
|
||||
cameraServer.setQuality(50);
|
||||
cameraServer.startAutomaticCapture("cam0");
|
||||
chooser.addDefault("Time based low bar", new RoutineDriveTime());
|
||||
chooser.addDefault("Nothing", new RoutineNothing());
|
||||
chooser.addObject("Time based low bar", new RoutineDriveTime());
|
||||
chooser.addObject("Time based defense", new RoutineDefenseTime());
|
||||
SmartDashboard.putData("Auto mode", chooser);
|
||||
SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController());
|
||||
|
@ -10,6 +10,7 @@ public class AutoSetArmPosition extends CommandBase {
|
||||
public AutoSetArmPosition(double p) {
|
||||
requires(mainArm);
|
||||
pos = p;
|
||||
setTimeout(3);
|
||||
}
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
@ -27,7 +28,7 @@ public class AutoSetArmPosition extends CommandBase {
|
||||
System.out.println("AutoSetArmPosition.isFinished(): " + (Math.abs(pos - mainArm.getDegrees()) <= 1));
|
||||
System.out.println(" pos : " + pos);
|
||||
System.out.println(" mainArm.getDegrees() : " + mainArm.getDegrees());
|
||||
return Math.abs(pos - mainArm.getDegrees()) <= 1;
|
||||
return (Math.abs(pos - mainArm.getDegrees()) <= 1) || isTimedOut();
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
|
@ -5,7 +5,7 @@ import org.usfirst.frc.team2059.robot.Robot;
|
||||
public class RoutineDefenseTime extends CommandGroup {
|
||||
public RoutineDefenseTime() {
|
||||
addSequential(new AutoSetArmStopState(false));
|
||||
addSequential(new AutoSetArmPosition(10));
|
||||
addSequential(new AutoDriveTime(2, .5));
|
||||
addSequential(new AutoSetArmPosition(6));
|
||||
addSequential(new AutoDriveTime(3, 1));
|
||||
}
|
||||
}
|
||||
|
@ -6,6 +6,6 @@ public class RoutineDriveTime extends CommandGroup {
|
||||
public RoutineDriveTime() {
|
||||
addSequential(new AutoSetArmStopState(true));
|
||||
addSequential(new AutoResetLower(-1));
|
||||
addSequential(new AutoDriveTime(2, .5));
|
||||
addSequential(new AutoDriveTime(3, 0.75));
|
||||
}
|
||||
}
|
||||
|
@ -0,0 +1,8 @@
|
||||
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 RoutineNothing extends CommandGroup {
|
||||
public RoutineNothing() {
|
||||
}
|
||||
}
|
@ -20,7 +20,7 @@ public class Drive extends CommandBase {
|
||||
if (Robot.oi.getJoysticks()[0].getRawButton(2) || Robot.oi.getJoysticks()[0].getRawButton(11)) {
|
||||
sensitivity = 1;
|
||||
} else {
|
||||
sensitivity = 0.5;
|
||||
sensitivity = 0.7;
|
||||
}
|
||||
driveBase.driveArcade(x, y, z, sensitivity);
|
||||
}
|
||||
|
@ -23,11 +23,6 @@ public class MoveArm extends CommandBase {
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
//TODO: I don't know if this should go here
|
||||
if (SmartDashboard.getBoolean("UseLimitSwitches")) {
|
||||
if (mainArm.getTopPressed() || mainArm.getBottomPressed()) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// Called once after isFinished returns true
|
||||
|
@ -15,7 +15,7 @@ public class MainArm extends PIDSubsystem {
|
||||
private double min = RobotMap.zeroDegrees;
|
||||
private double max = RobotMap.ninetyDegrees;
|
||||
public MainArm() {
|
||||
super("MainArm", 0.07, 0.0, 0.00);
|
||||
super("MainArm", 0.3, 0.0, 0.4);
|
||||
getPIDController().setContinuous(false);
|
||||
setSetpoint(70);
|
||||
enable();
|
||||
|
Loading…
Reference in New Issue
Block a user