public void autonomousPeriodic() {
relay.set(Relay.Value.kOn);
smart.putNumber("distance", ultrasonic.getVoltage());
drive.setRun(false);
loadAndShoot.setRun(false);
// relay.set(Relay.Value.kOn);
if (autoTimer.get() < autoMoveForwardTime) {
System.out.println(
"Moving forward, Timer at "
+ autoTimer.get()
+ ", Ultrasonic at "
+ ultrasonic.getAverageVoltage());
}
if (autoTimer.get() >= autoMoveForwardTime && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
stop();
if (!autonomousStopped) {
autonomousStopped = true;
autoTimeAtStop = autoTimer.get();
}
System.out.println("Stopped, Ultrasonic at " + ultrasonic.getAverageVoltage());
}
if (autoTimer.get() >= autoTimeAtStop + 1 && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
autoStop = true;
shoot();
System.out.println("Shooting, Ultrasonic at " + ultrasonic.getAverageVoltage());
}
}
public TargetFinder() {
System.out.println("TargetFinder() begin " + Timer.getFPGATimestamp());
cam = AxisCamera.getInstance();
cam.writeResolution(AxisCamera.ResolutionT.k320x240);
cam.writeBrightness(camBrightness);
cam.writeColorLevel(camColor);
cam.writeWhiteBalance(camWhiteBalance);
cam.writeExposureControl(camExposure);
cam.writeMaxFPS(15);
cam.writeExposurePriority(AxisCamera.ExposurePriorityT.none);
cam.writeCompression(50);
// System.out.println("TargetFinder() * " + cam.toString() + "[" + Timer.getFPGATimestamp());
boxCriteria = new CriteriaCollection();
inertiaCriteria = new CriteriaCollection();
boxCriteria.addCriteria(
NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, bboxWidthMin, false);
boxCriteria.addCriteria(
NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, bboxHeightMin, false);
inertiaCriteria.addCriteria(
NIVision.MeasurementType.IMAQ_MT_NORM_MOMENT_OF_INERTIA_XX, 0, inertiaXMin, true);
inertiaCriteria.addCriteria(
NIVision.MeasurementType.IMAQ_MT_NORM_MOMENT_OF_INERTIA_YY, 0, inertiaYMin, true);
Timer.delay(7);
}
/*
* reads the value of the digital input of the specified channel
* requires that the readArudiono() method has already been called
* param 1: the channel to be read
* return: value of the specified digital input
*/
public boolean getInput(int channelID) {
byte temp;
if (timer.get() > 2.0) {
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser3, 1, " ");
DriverStationLCD.getInstance().updateLCD();
timer.stop();
timer.reset();
}
if (Math.abs(channelID - (13 / 2)) - (13 / 2)
> 0) // returns true if less than zero or greater than thirteen
{
timer.start();
DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1, "Invalid ID");
DriverStationLCD.getInstance().updateLCD();
return false;
} else if (channelID < 8) {
temp = (byte) (loByte & (1 << channelID));
return temp != 0;
} else {
temp = (byte) (hiByte & (1 << channelID - 8));
return temp != 0;
}
}
public void autonomousInit() {
try {
robot.arm.moveTo(PegPosition.RESET);
} catch (CANTimeoutException e1) {
// TODO Auto-generated catch block
e1.printStackTrace();
}
robot.compressor.start();
Timer.delay(3.5);
robot.grabber.tiltDown();
Timer.delay(2);
robot.grabber.grab();
robot.lineSensor.setLinePreference(LinePreference.LEFT);
controller.enable();
Timer.delay(2.5);
robot.grabber.tiltUp();
Timer.delay(2.5);
try {
robot.arm.moveTo(PegPosition.MIDDLE_OFFSET);
} catch (CANTimeoutException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
protected void initialize() {
time = new Timer();
time.reset();
time.start();
}
public void driveToBridge() {
System.out.println("Val: " + potentiometer.getVoltage());
if (Autonomousflag) {
if (potentiometer.getVoltage() < 5) { // line
shoot();
Timer.delay(7);
shoot.set(0);
collect.set(0);
driveTrain.tankDrive(-speed, speed);
Timer.delay(1);
driveTrain.tankDrive(speed, speed);
Timer.delay(1.2);
driveTrain.tankDrive(0, 0);
Timer.delay(0.9);
bridge.set(-1);
} else if (potentiometer.getVoltage() > 5) { // left
shoot();
}
Autonomousflag = false;
} else {
driveTrain.tankDrive(0, 0);
}
}
public boolean waitForComplete(double milli) {
System.out.println("time currently: " + timer.get() + " - waiting for: " + milli);
if (timer.get() <= milli) return false;
else {
return true;
}
}
@Override
protected void initialize() {
beginTime = Integer.MAX_VALUE;
triggered = false;
timer1.reset();
timer1.start();
System.out.println("Shoot Init");
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.logger.log("Initialize", 1);
finished = false;
timer.reset();
timer.start();
lastOffset = 0;
stillCycles = 0;
}
public void shoot(double timeToShoot) {
m_timeToShoot = timeToShoot;
if (false == shooting && false == retracting) {
shooting = true;
retracting = false;
shootTime.start();
shootTime.reset();
}
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.drivetrain.resetGyro();
Robot.drivetrain.drivePID.setSetpoint(STRAIGHT_SET_POINT);
Robot.drivetrain.throttleInt.useJoystick(false);
Robot.drivetrain.throttleInt.setThrottle(thisDriveSpeed);
timer2.start();
startTime2 = timer2.get();
state = State.START_DRIVE; // initializes the state
}
protected void execute() {
double time = _timer.get();
if (time >= _period) {
Robot.ledStrip.setColor(
_colors[index].getRed(), _colors[index].getGreen(), _colors[index].getBlue());
index = (index + 1) % _colors.length;
_timer.reset();
}
}
public double getRPM() {
if (Timer.getFPGATimestamp() > RPMUpdatePeriod + encoderMarkTime) {
double encoderCPS =
(enc.get() - encoderMarkCounts) / (Timer.getFPGATimestamp() - encoderMarkTime);
encoderRPM = (encoderCPS * 60) / 16;
encoderMarkCounts = enc.get();
encoderMarkTime = Timer.getFPGATimestamp();
}
return encoderRPM;
}
protected void initialize() {
if (!inProgress) {
inProgress = true;
Pickup.checkArms();
Catapult.latch.set(Latch.UNLOCKED);
Timer.delay(0.5);
Catapult.setLauncher(RETRACTED);
Timer.delay(2.0);
Catapult.latch.set(Latch.LOCKED);
}
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.intake.setIntakePosition(true);
Timer.delay(1);
Robot.shooter.setShooterPosition(true);
Robot.intake.setIntakePosition(false);
Timer.delay(4);
// Robot.shooter.switchShooterPosition();
// runTime.reset();
// runTime.start();
System.out.println("rock wall");
}
/** Resets the recorder so we can record properly next time */
public void reset() {
if (m_bRecStarted) {
writeDataToFile();
m_List.removeAllElements();
m_Index = 0;
m_tmRecorder.stop();
m_tmRecorder.reset();
m_bRecDone = false;
m_bRecStarted = false;
}
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.driveTrain.setLeftMotors(0.5);
Robot.driveTrain.setRightMotors(-0.5);
Timer.delay(1.5);
Robot.driveTrain.setLeftMotors(0);
Robot.driveTrain.setRightMotors(-0.8);
Timer.delay(0.5);
Robot.driveTrain.setLeftMotors(0.6);
Robot.driveTrain.setRightMotors(-0.5);
Timer.delay(1);
}
public void autonomousInit() {
// Timer section
timerMain = new Timer();
timerMain.reset();
timerMain.start();
timer1Left = new Timer();
timer2Left = new Timer();
timer1Right = new Timer();
timer2Right = new Timer();
timer1Center = new Timer();
timer2Center = new Timer();
controlChuteLeft = new boolean[] {false, false};
controlChuteRight = new boolean[] {false, false};
controlChuteCenter = new boolean[] {false, false};
controlChuteLeft[0] = true;
controlChuteRight[0] = true;
controlChuteCenter[0] = true;
/* Boolean setting section
* Robot Aligns Left
*/
if (DigitalInput7.get() == false && DigitalInput8.get() == true) {
b_leftAutonomous = true;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = true;
b_leftDriveAutonomous = false;
}
// Robot Aligns Right
if (DigitalInput7.get() == true && DigitalInput8.get() == false) {
b_leftAutonomous = false;
b_rightAutonomous = true;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = true;
b_rightDriveAutonomous = false;
}
// Robot Aligns Center
if (DigitalInput7.get() == true && DigitalInput8.get() == true) {
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = true;
b_upAutonomous = false;
b_downAutonomous = true;
b_centerDriveAutonomous = false;
}
} // End autonomousInit
private double m_shoot() {
if (encoder.get() >= desired || shootTime.get() > m_timeToShoot) {
shooting = false;
retracting = true;
shootTime.stop();
retractTime.start();
retractTime.reset();
return .1;
} else { // encoder < desired && shootTime < 0.2
error = desired - encoder.get();
return -1;
}
}
protected void execute() {
double delta = timer.get();
distance += (drivetrain.getAverageForwardVelocity() * delta);
timer.reset();
// PID tuning code
pidControllerDistance.setPID(
pidConfigDistance.getP(P),
pidConfigDistance.getI(I),
pidConfigDistance.getD(D),
pidConfigDistance.getF(F));
pidConfigDistance.setSetpoint(pidControllerDistance.getSetpoint());
pidConfigDistance.setValue(drivetrainRotation.pidGet());
}
protected void execute() {
if (time.get() < 3.0) {
talonLeft.set(1);
talonRight.set(1);
} else if (time.get() < 5.0) {
talonLeft.set(0);
talonRight.set(0);
soleLiftDown.set(true);
} else if (time.get() < 6.0) {
talonLeft.set(-1);
talonRight.set(-1);
} else talonLeft.set(1);
talonRight.set(1);
}
/** Runs the motors with tank steering. */
public void operatorControl() {
myRobot.setSafetyEnabled(true);
while (isOperatorControl() && isEnabled()) {
myRobot.tankDrive(leftStick, rightStick);
Timer.delay(0.005); // wait for a motor update time
}
}
@Override
protected void end() {
timer1.stop();
shooterWheel.rawShoot(0);
shooterBar.rawBallPush(0);
System.out.println("Shoot End");
}
protected void end() {
Timer.delay(1.5);
if (Catapult.latch.get() == Latch.UNLOCKED) {
Catapult.latch.set(Latch.LOCKED); // set latch in
}
inProgress = false;
}
public AutoCommandGroup() {
// STARTING AT YELLOW BIN
addSequential(new LiftCommand(0.75, 0.5, true));
Timer.delay(1);
addSequential(new AutoMove(1, 2.5, 13.5));
addParallel(new LiftCommand(1, 0.5, true));
addSequential(new LiftCommand(0.75, 0.5, false));
// STARTING AT LANDFILL
// addSequential(new AutoMove (0.6, 2.5, 0.4));
// Timer.delay(1.5);
// addSequential(new LiftCommand (0.75,0.5, true));
// addSequential(new AutoMove (-1, 2.5, 7.8));
// addSequential (new LiftCommand (0.75, 0.5, false));
// Add Commands here:
// e.g. addSequential(new Command1());
// addSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use addParallel()
// e.g. addParallel(new Command1());
// addSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
}
public Shooter(int encoderA, int encoderB, int victor1, int victor2, int victor3) {
encoder = new Encoder(encoderA, encoderB);
encoder.start();
encoder.reset();
motor1 = new Victor(victor1); // 2
motor2 = new Victor(victor2); // 5
motor3 = new Victor(victor3); // 4
time = new Timer();
shootTime = new Timer();
retractTime = new Timer();
time.start();
time.reset();
}
/**
* start up automatic capture you should see the video stream from the webcam in your FRC PC
* Dashboard.
*/
public void operatorControl() {
while (isOperatorControl() && isEnabled()) {
/** robot code here! * */
Timer.delay(0.005); // wait for a motor update time
}
}
@Override
protected void initialize() {
backLeft = new Talon(ControlsManager.BACK_LEFT_SPEED_CONTROLLER);
frontLeft = new Talon(ControlsManager.FRONT_LEFT_SPEED_CONTROLLER);
backRight = new Talon(ControlsManager.BACK_RIGHT_SPEED_CONTROLLER);
frontRight = new Talon(ControlsManager.FRONT_RIGHT_SPEED_CONTROLLER);
backLeft.setInverted(true);
frontLeft.setInverted(true);
backRight.setInverted(true);
frontRight.setInverted(true);
lidar = new LIDAR(Port.kMXP);
// ultrasonic = new Ultrasonic(RobotMap.ultraPing, RobotMap.ultraEcho);
// ultrasonic.setEnabled(true); ultrasonic.setAutomaticMode(true);
maxbotix = new BadUltrasonic(ControlsManager.MAXBOTIX_ULTRASONIC);
// mxp stuff
serialPort = new SerialPort(57600, SerialPort.Port.kMXP);
byte update_rate_hz = 127;
mxp = new IMU(serialPort, update_rate_hz);
Timer.delay(0.3);
mxp.zeroYaw();
train = new RobotDrive(backLeft, frontLeft, backRight, frontRight);
}
public void calibrateAcel() {
double avgX = 0;
double avgY = 0;
for (int i = 0; i < calibrationSamples; i++) {
avgX += acel.getX();
avgY += acel.getY();
Timer.delay(0.005); // calibrationSampleRate / 1000.0);
}
avgX /= calibrationSamples;
avgY /= calibrationSamples;
acelBias = new Vector2(avgX, avgY);
lastTimestamp = Timer.getFPGATimestamp();
// SmartDashboard.putNumber("X Bias", acelBias.x);
// SmartDashboard.putNumber("Y Bias", acelBias.y);
}
public void init() {
try {
TKOHardware.changeTalonMode(
TKOHardware.getLeftDrive(), CANTalon.ControlMode.Position, p, i, d);
TKOHardware.changeTalonMode(
TKOHardware.getRightDrive(), CANTalon.ControlMode.Position, p, i, d);
TKOHardware.getLeftDrive().reverseOutput(false);
TKOHardware.getRightDrive().reverseOutput(true);
TKOHardware.getLeftDrive().reverseSensor(true);
TKOHardware.getRightDrive().reverseSensor(false);
TKOHardware.getLeftDrive().enableBrakeMode(true);
TKOHardware.getRightDrive().enableBrakeMode(true);
TKOHardware.getLeftDrive().setPosition(0); // resets encoder
TKOHardware.getRightDrive().setPosition(0);
TKOHardware.getLeftDrive().ClearIaccum(); // stops bounce
TKOHardware.getRightDrive().ClearIaccum();
Timer.delay(0.1);
TKOHardware.getLeftDrive().set(TKOHardware.getLeftDrive().getPosition());
TKOHardware.getRightDrive().set(TKOHardware.getRightDrive().getPosition());
TKOHardware.getPiston(0).set(Definitions.SHIFTER_LOW);
TKOHardware.getPiston(2).set(Definitions.WHEELIE_RETRACT);
} catch (TKOException e) {
e.printStackTrace();
System.out.println("Err.... Talons kinda died ");
}
System.out.println("Drive atom initialized");
}
