public void disabledPeriodic() {
// gyroSubAvg += gyro.getAngle();
// gyroSubAvg /= 2;
SmartDashboard.putNumber("Gyro", gyro.getAngle());
// SmartDashboard.putNumber("Gyro Scaling Value", gyroSubAvg);
}
/** The log method puts interesting information to the SmartDashboard. */
public void log() {
SmartDashboard.putNumber("Left Distance", left_encoder.getDistance());
SmartDashboard.putNumber("Right Distance", right_encoder.getDistance());
SmartDashboard.putNumber("Left Speed", left_encoder.getRate());
SmartDashboard.putNumber("Right Speed", right_encoder.getRate());
SmartDashboard.putNumber("Gyro", gyro.getAngle());
}
public void robotInit() {
j1 = new Joystick(1);
j2 = new Joystick(0);
shooter = new Victor(2);
shooter2 = new Victor(4);
intake = new Victor(3);
right = new Victor(0);
left = new Victor(1);
rd = new RobotDrive(left, right);
light = new DigitalInput(3);
count = 0;
comp = new Compressor();
inup = new DoubleSolenoid(0, 1);
inup2 = new DoubleSolenoid(2, 3);
gyro = new AnalogGyro(0);
gyro.setSensitivity(.007);
gyro.calibrate();
gyro.reset();
enc = new Encoder(1, 2, true, Encoder.EncodingType.k4X);
enc.setDistancePerPulse(.11977); // circumference of wheel/200 (PPR)
enc.setPIDSourceType(PIDSourceType.kDisplacement);
enc.reset();
out =
new PIDOutput() {
@Override
public void pidWrite(double out) {
rd.setLeftRightMotorOutputs(out, out);
}
};
controller = new PIDController(.007, 0, 0, enc, out);
controller.setAbsoluteTolerance(.25);
controller.disable();
gcontroller = new PIDController(kp, ki, kd, gyro, out);
gcontroller.setAbsoluteTolerance(ktol);
gcontroller.disable();
timerStart = -1;
timeToCancel = -1;
}
public static void init() {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
driveTrainLeftFront = new Talon(0);
LiveWindow.addActuator("DriveTrain", "Left Front", (Talon) driveTrainLeftFront);
driveTrainLeftRear = new Talon(1);
LiveWindow.addActuator("DriveTrain", "Left Rear", (Talon) driveTrainLeftRear);
driveTrainRightFront = new Talon(2);
LiveWindow.addActuator("DriveTrain", "Right Front", (Talon) driveTrainRightFront);
driveTrainRightRear = new Talon(3);
LiveWindow.addActuator("DriveTrain", "Right Rear", (Talon) driveTrainRightRear);
driveTrainHolonomicDrive =
new RobotDrive(
driveTrainLeftFront, driveTrainLeftRear, driveTrainRightFront, driveTrainRightRear);
driveTrainHolonomicDrive.setSafetyEnabled(false);
driveTrainHolonomicDrive.setExpiration(0.1);
driveTrainHolonomicDrive.setSensitivity(0.5);
driveTrainHolonomicDrive.setMaxOutput(1.0);
driveTrainHolonomicDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
driveTrainHolonomicDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
driveTrainGyro = new AnalogGyro(0);
LiveWindow.addSensor("DriveTrain", "Gyro", driveTrainGyro);
driveTrainGyro.setSensitivity(0.007);
forkliftMotor = new Talon(4);
LiveWindow.addActuator("Forklift", "Motor", (Talon) forkliftMotor);
forkliftEncoder = new Encoder(0, 1, false, EncodingType.k4X);
LiveWindow.addSensor("Forklift", "Encoder", forkliftEncoder);
forkliftEncoder.setDistancePerPulse(0.012);
forkliftEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
toteCaptureLeftWheel = new Talon(8);
LiveWindow.addActuator("Tote Capture", "Left Wheel", (Talon) toteCaptureLeftWheel);
toteCaptureRightWheel = new Talon(9);
LiveWindow.addActuator("Tote Capture", "Right Wheel", (Talon) toteCaptureRightWheel);
containerCaptureMotor = new Talon(10);
LiveWindow.addActuator("Container Capture", "Motor", (Talon) containerCaptureMotor);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
powerDistributionPanel = new PowerDistributionPanel();
}
public void teleopPeriodic() {
/*SmartDashboard.getNumber("Accel x: ", accel.getX());
SmartDashboard.getNumber("Accel Y: ", accel.getY());
SmartDashboard.getNumber("Accel Z: ", accel.getZ());*/
if (controller.isEnabled()) {
if (((Timer.getFPGATimestamp() - timerStart) > timeToCancel) || controller.onTarget()) {
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
}
if (j1.getRawButton(2)) {
enc.reset();
controller.setSetpoint(120); // destination 24 inches -> NO!! Trying to figure out this value
timerStart = Timer.getFPGATimestamp();
timeToCancel = 10; // timeout after 10 seconds
controller.enable();
} else if (j1.getRawButton(
1)) { // this button stops the robot if the button 2 command goes crazy
controller.disable();
timerStart = -1;
timeToCancel = -1;
} else { // if time out or distance, end
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
if (!controller.isEnabled()) {
rd.arcadeDrive(-j1.getY(), -j1.getX()); // normal arcade drive
}
if (j2.getRawAxis(2) != 0) { // set shooter values to the left trigger value
shooter.set(-j2.getRawAxis(2));
shooter2.set(-j2.getRawAxis(2));
SmartDashboard.putNumber("Shooter: ", shooter.get());
} else { // stop shooter
shooter.set(0);
shooter2.set(0);
}
/*if(j2.getRawButton(8)){ //runs intake, waits, runs shooter
shooter.set(-1);
intake.set(.5);
Timer.delay(.50);
intake.set(0);
Timer.delay(1.5);
intake.set(-1);
Timer.delay(1);
shooter.set(0);
intake.set(0);
}*/
// Need to mess around with sensitivity of light sensor
if (j2.getRawAxis(3) != 0 && !light.get()) { // run intake at speed of right trigger
intake.set(-1 * j2.getRawAxis(3));
} else if (j2.getRawButton(5) && !light.get()) { // run intake into robot
intake.set(-1);
} else if (j2.getRawButton(6)) { // run intake out of robot
intake.set(1);
} else { // stop intake
intake.set(0);
}
if (j2.getRawButton(2)) { // lift intake
inup.set(DoubleSolenoid.Value.kForward);
inup2.set(DoubleSolenoid.Value.kForward);
} else if (j2.getRawButton(3)) { // drop intake
inup.set(DoubleSolenoid.Value.kReverse);
inup2.set(DoubleSolenoid.Value.kReverse);
} else { // solenoids off
inup.set(DoubleSolenoid.Value.kOff);
inup2.set(DoubleSolenoid.Value.kOff);
}
// reading values
SmartDashboard.putNumber("Encoder Dist: ", enc.getDistance()); // Distance is in inches
SmartDashboard.putNumber("Encoder: ", enc.get());
SmartDashboard.putNumber("Encoder Rate: ", enc.getRate());
SmartDashboard.putNumber("Gyro: ", gyro.getAngle());
SmartDashboard.putNumber("Encoder Raw", enc.getRaw());
SmartDashboard.putNumber("Controller: ", controller.getSetpoint());
SmartDashboard.putBoolean("Light Sensor: ", light.get());
}
public void teleopInit() {
gyro.reset();
enc.reset();
}
public void disabledInit() {
gyroSubAvg = 0;
gyro.reset();
}
/** @return The robots heading in degrees. */
public double getHeading() {
return gyro.getAngle();
}
/** Reset the robots sensors to the zero states. */
public void reset() {
gyro.reset();
left_encoder.reset();
right_encoder.reset();
}
