// Called just before this Command runs the first time
protected void initialize() {
if (SmartDashboard.getNumber("centerAge", 1) < 0.5)
angle = SmartDashboard.getNumber("angleToCenter", 0);
else angle = 0;
super.initialize();
// System.out.println("Angle to Center: " + angle);
}
public DriveAtom(double _dist) {
p = SmartDashboard.getNumber("Drive P: ");
i = SmartDashboard.getNumber("Drive I: ");
d = SmartDashboard.getNumber("Drive D: ");
distance = _dist;
incrementer = Definitions.DRIVE_ATOM_INCREMENTER;
threshold = 75; // we can be within approx. half an inch
}
@Override
public void sendToSmartDash() {
SmartDashboard.putNumber(this.getName() + " kP", kP);
SmartDashboard.putNumber(this.getName() + " kI", kI);
SmartDashboard.putNumber(this.getName() + " kD", kD);
SmartDashboard.putNumber(this.getName() + " ERROR", this.previousError);
kP = SmartDashboard.getNumber(this.getName() + " kP", kP);
kI = SmartDashboard.getNumber(this.getName() + " kI", kI);
kD = SmartDashboard.getNumber(this.getName() + " kD", kD);
}
// Called just before this Command runs the first time
protected void initialize() {
System.out.println("Initialize timedPullBackHammer");
setTimeout(SmartDashboard.getNumber("Timed Hammer Pullback"));
if (!Robot.hammer.isHammerAtLatch()) {
System.out.println(
"Pulling back hammer for " + SmartDashboard.getNumber("Timed Hammer Pullback") + " sec");
if (Robot.hammer.isLatchReady()) Robot.hammer.releaseLatch();
Robot.hammer.pullBackHammer((float) SmartDashboard.getNumber("Hammer Motor Speed"));
} else {
System.out.println("Error: hammer is at latch");
}
}
public void teleopPeriodic() {
armpid.setPID(
SmartDashboard.getNumber("kP"),
SmartDashboard.getNumber("kI"),
SmartDashboard.getNumber("kD"));
if (joy1.getRawButton(1)) {
armpid.enable();
} else {
armpid.disable();
arm.set(0);
}
SmartDashboard.putBoolean("Checkbox 2", limit.get());
System.out.println("limit " + limit.get());
}
/*
* Code to support update of PIDF constants from a button on the SmartDashboard
*/
public void updatePIDF() {
// Assign
double p = SmartDashboard.getNumber(m_name + " P");
double i = SmartDashboard.getNumber(m_name + " I");
double d = SmartDashboard.getNumber(m_name + " D");
if (m_hasFeedForward) {
double f = SmartDashboard.getNumber(m_name + " F");
setPID(p, i, d, f);
} else setPID(p, i, d);
// Update Tolerance
setTolerance(SmartDashboard.getNumber(m_name + " Tolerance"));
}
public Double getDouble(String key) {
try {
return Double.valueOf(SmartDashboard.getNumber(key));
} catch (TableKeyNotDefinedException e) {
return null;
}
}
// Called just before this Command runs the first time
protected void initialize() {
if (system == System.DRIVEDISTANCE) {
target = SmartDashboard.getNumber("PID/DriveDistance TestTarget");
Robot.drivetrain.setAutoTarget(target, 0, 0);
} else if (system == System.DRIVESLIDE) {
target = SmartDashboard.getNumber("PID/DriveSlide TestTarget");
Robot.drivetrain.setAutoTarget(0, target, 0);
} else if (system == System.DRIVEANGLE) {
target = SmartDashboard.getNumber("PID/DriveAngle TestTarget");
Robot.drivetrain.setAutoTarget(0, 0, target);
} else if (system == System.ELEVATOR) {
target = SmartDashboard.getNumber("PID/Elevator TestTarget");
Robot.elevator.setAutoTarget(target);
// } else if (system == System.ARM) {
// target = SmartDashboard.getNumber("PID/Arm TestTarget");
// SlideWinder.arm.setArmHeight(target);
}
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
deadbandY = SmartDashboard.getNumber("DeadbandY");
deadbandX = SmartDashboard.getNumber("DeadbandX");
deadbandZ = SmartDashboard.getNumber("DeadbandZ");
power = (int) SmartDashboard.getNumber("DelinPower");
double leftspeed = getLeft();
double rightspeed = getRight();
if (currentControlMode == CANTalon.TalonControlMode.Speed.getValue()) {
Robot.driveTrain.setLeftSpeedPercentage(leftspeed);
Robot.driveTrain.setRightSpeedPercentage(rightspeed);
} else {
Robot.driveTrain.setLeftPercentVBus(leftspeed);
Robot.driveTrain.setRightPercentVBus(rightspeed);
}
Robot.driveTrain.writeDashboardDebugValues();
}
/**
* Spin wheel at desired feet per second Uses PID loop for consistency
*
* @param feetPerSecond double
*/
public void spin(double feetPerSecond) {
double desiredSpeed = feetPerSecond;
pidData.kP = SmartDashboard.getNumber("Right flywheel kP: ");
pidData.kI = SmartDashboard.getNumber("Right flywheel kI: ");
pidData.kD = SmartDashboard.getNumber("Right flywheel kD: ");
if (desiredSpeed == 0) {
motor.set(0);
return;
}
double currentSpeed = 0;
double error = desiredSpeed - currentSpeed;
integral += error;
derivative = error - previousError;
if (Math.abs(error) < TOLERANCE_FEET_PER_SECOND) {
integral = 0;
}
output += (error * pidData.kP) + (integral * pidData.kI) + (derivative * pidData.kD);
if (output > 1) {
output = 1;
} else if (output < -1) {
output = -1;
}
motor.set(feetPerSecond);
for (int i = (errors.length - 1); i > 0; i--) {
errors[i] = errors[i - 1];
}
errors[0] = error;
previousError = error;
// System.out.println("Output: " + output);
// System.out.println("Error: " + error);
// System.out.println("kP: "+ pidData.kP);
}
public void update(GoalAlign alignment) {
double shooterPower;
if (SmartDashboard.getBoolean("Flywheel Testing Mode") == false) {
shooterPower = 1;
} else {
shooterPower = SmartDashboard.getNumber("Flywheel Testing Power");
}
if (gamePad.buttons.BUTTON_A.isOn()) {
// shoot(DistanceSensorData);
// shooter.getMotor().getSpark().set(1);
// shooter.shoot(100);s
// shooter.setPower(getPowerFromDistance(alignment.imageHeight, alignment.centerY,
// alignment.height));
shooter.setPower(shooterPower);
} else if (!gamePad.buttons.BUTTON_A.isOn()) {
shooter.setPower(0);
}
SmartDashboard.putBoolean("Shooting", gamePad.buttons.BUTTON_A.isOn());
}
public double get() {
return SmartDashboard.getNumber(key, defaultValue);
}
public AutoState getState(IRobot robot) {
int chooser = (int) SmartDashboard.getNumber("Autonomous", 1);
boolean backwards = SmartDashboard.getBoolean("Autonomous Backwards", false);
AutoState chain = new NothingAutonomous();
switch (chooser) {
// TODO: Refine and test the state chains.
case 1:
if (backwards) {
chain
.addNext(new ForwardAutonomous(1000, -speed))
.addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
} else {
chain.addNext(new ForwardAutonomous(1000, speed));
}
// FieldPosition.Left
chain
.addNext(new AngleAutonomous(90.0, 0.0, robot.getSensorData().getGyro()))
// TODO: Determine if positive means clockwise.
.addNext(null);
// TODO: Replace with an Autonomous that drives forward until it reaches the goal.
case 2:
if (backwards) {
chain
.addNext(new ForwardAutonomous(1000, -speed))
.addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
} else {
chain.addNext(new ForwardAutonomous(1000, speed));
}
// FieldPosition.Left
chain
.addNext(new AngleAutonomous(90.0, 0.0, robot.getSensorData().getGyro()))
.addNext(null);
case 3:
if (backwards) {
chain
.addNext(new ForwardAutonomous(1000, -speed))
.addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
} else {
chain.addNext(new ForwardAutonomous(1000, speed));
}
// FieldPosition.Left
chain
.addNext(new AngleAutonomous(90.0, 0.0, robot.getSensorData().getGyro()))
.addNext(null);
case 4:
// This is right in front of the tower. What is our procedure for this?
if (backwards) {
chain
.addNext(new ForwardAutonomous(1000, -speed))
.addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
} else {
chain.addNext(new ForwardAutonomous(1000, speed));
}
// FieldPosition.Right
chain
.addNext(new AngleAutonomous(-90.0, 0.0, robot.getSensorData().getGyro()))
.addNext(null);
case 5:
if (backwards) {
chain
.addNext(new ForwardAutonomous(1000, -speed))
.addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
} else {
chain.addNext(new ForwardAutonomous(1000, speed));
}
// FieldPosition.Right
chain
.addNext(new AngleAutonomous(-90.0, 0.0, robot.getSensorData().getGyro()))
.addNext(null);
default:
// Just nothing
}
return chain;
}
// returns a double at the position of the key
public double getNumber(String key) {
return board.getNumber(key);
}
// Whoopystick
// dudepiston is pneumatic pusher
public class Shooter {
public int shootingMotor = 6;
public int shootingMotor2 = 2;
public Victor motorOne = new Victor(6);
public Victor motorTwo = new Victor(2);
double maxspeed;
double angle;
double speeddif;
double speed = 0;
double testValueOne = SmartDashboard.getNumber("testValueOne");
double testValueTwo = SmartDashboard.getNumber("testValueTwo");
OI oi;
CANJaguar Shrek;
double speedmultOne = 0; // goal side wheel
double speedmultTwo = 0; // feeder side wheel
public CANJaguar tester1;
public CANJaguar tester2;
public CANJaguar tester3;
public CANJaguar tester4;
public double magOne;
public double magTwo;
public double speedOne;
public double speedTwo;
public double tspeedTwo;
public double speedThree;
public double speedFour;
public double deadZone = .1;
CANJaguar.ControlMode controlMode = CANJaguar.ControlMode.kVoltage; // Voltage drive
CANJaguar.NeutralMode neutralMode2 = CANJaguar.NeutralMode.kBrake;
CANJaguar.NeutralMode neutralMode = CANJaguar.NeutralMode.kCoast;
CANJaguar.ControlMode controlModew = CANJaguar.ControlMode.kSpeed;
// NetworkTable table = NetworkTable.getTable("camera");
public double distance;
public Shooter(OI oi) {
this.oi = oi;
initializeOne();
initializeTwo();
// NetworkTable.setTeam(1259);
// NetworkTable.setIPAddress("10.12.59.2");
// distance = table.getDouble("distance");
}
public void initializeOne() {
try {
tester1 = new CANJaguar(shootingMotor, controlMode);
tester1.configNeutralMode(neutralMode);
// FrontLeftJJ.setPID(kP, kI, kD);
// FrontLeftJJ.setVoltageRampRate(rampRate);
// BackLeftJJ.setVoltageRampRate(rampRate);
// FrontRightJJ.setVoltageRampRate(rampRate);
// BackRightJJ.setVoltageRampRate(rampRate);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void initializeTwo() {
try {
tester2 = new CANJaguar(shootingMotor2, controlMode);
tester2.configNeutralMode(neutralMode);
// FrontLeftJJ.setPID(kP, kI, kD);
// FrontLeftJJ.setVoltageRampRate(rampRate);
// BackLeftJJ.setVoltageRampRate(rampRate);
// FrontRightJJ.setVoltageRampRate(rampRate);
// BackRightJJ.setVoltageRampRate(rampRate);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void voltageChange() {
// 3 point goal
if (this.oi.getY()) { // shoot mid
speedmultOne = 10.6; // 10.1 8.7
speedmultTwo = 8.2; // 2.16
} // 2 point goal
else if (this.oi.getX()) {
speedmultOne = 10.4; // 10.7 8.2
speedmultTwo = 7.9;
} // pooter speed
else if (this.oi.getB()) {
speedmultOne = 4.0;
speedmultTwo = 7.5;
} else if (this.oi.getLJoystick().getRawButton(10)) {
speedmultOne = testValueOne;
speedmultTwo = testValueTwo;
}
}
public void autoShootThree() {
try {
tester1.setX(10.8); // 9.5 8.2 2 pt
tester2.setX(8.3);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void autoShoot() {
try {
tester1.setX(10.3); // 9.5 8.2 2 pt
tester2.setX(7.8);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void autoShootMid() { // shoot middle
try {
tester1.setX(10.5);
tester2.setX(8.0); // 9.2
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void autoShootThreeSide() { // shoot middle
try {
tester1.setX(10.6);
tester2.setX(8.5);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void drive() {
try {
magOne = this.oi.getXJoystick().getY();
if (Math.abs(magOne) < deadZone) {
magOne = 0;
}
if (Math.abs(magTwo) < deadZone) {
magTwo = 0;
}
speedOne = magOne * speedmultOne; // 4500
speedTwo = magTwo * speedmultTwo; // 14000
tspeedTwo = magOne * speedmultTwo;
tester1.setX(-speedOne);
tester2.setX(-tspeedTwo);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void testValue() {
if (oi.getLJoystick().getTrigger()) {
boolean distcheck = NetworkTable.getTable("camera").containsKey("distance");
NetworkTable.getTable("rob").putNumber("x", 8008135);
System.out.println(distcheck);
if (distcheck) {
distance = NetworkTable.getTable("camera").getNumber("distance");
}
System.out.println("Distance:" + " " + distance);
}
}
}
