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 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();
}
}
// 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);
}
// 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");
}
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);
}
}
/** Displays test pattern of all defined characters. */
public void displayTestPattern() {
byte[] byte1 = new byte[10];
Util.consoleLog();
// first reset the transmit array to zeros.
for (int c = 0; c < 10; c++) {
byte1[c] = (byte) (0b00000000) & 0xFF;
}
// put single character data in the array and write to display.
for (int c = 0; c < 9; c++) {
byte1[0] = (byte) (0b0000111100001111);
byte1[2] = charreg[4 * c + 3][0];
byte1[3] = charreg[4 * c + 3][1];
byte1[4] = charreg[4 * c + 2][0];
byte1[5] = charreg[4 * c + 2][1];
byte1[6] = charreg[4 * c + 1][0];
byte1[7] = charreg[4 * c + 1][1];
byte1[8] = charreg[4 * c][0];
byte1[9] = charreg[4 * c][1];
// send the array to the board
i2c.writeBulk(byte1);
Timer.delay(3);
}
}
/**
* 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
}
}
/** 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
}
}
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 void testCounter() {
double counter = 0.0;
while (true) {
SmartDashboard.putNumber("Counter", counter++);
Timer.delay(.10);
}
}
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");
}
protected void end() {
Timer.delay(1.5);
if (Catapult.latch.get() == Latch.UNLOCKED) {
Catapult.latch.set(Latch.LOCKED); // set latch in
}
inProgress = false;
}
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);
}
@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);
}
/** Gyro sensitivity is set and mecanum drive is used with the gyro angle as an input. */
public void operatorControl() {
gyro.setSensitivity(
voltsPerDegreePerSecond); // calibrate gyro to have the value equal to degrees
while (isOperatorControl() && isEnabled()) {
myRobot.mecanumDrive_Cartesian(
joystick.getX(), joystick.getY(), joystick.getZ(), gyro.getAngle());
Timer.delay(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
public void turnStart(double degrees) {
turnPID.reset();
gyro.reset();
turnPID.setSetpoint(degrees);
turnPID.enable();
Timer.delay(.1);
System.out.println(
"start turning from " + RobotMap.roundtoTwo(gyro.getAngle()) + " to setpoint " + degrees);
}
protected void execute() {
if (!inProgress) {
inProgress = true;
if (Catapult.latch.get() == Latch.LOCKED) {
Catapult.latch.set(Latch.UNLOCKED);
Timer.delay(0.2);
}
if (pwmMode) {
Pickup.checkArms();
Catapult.setLauncher(EXTENDED);
Timer.delay(0.2);
Catapult.setLauncher(RETRACTED);
Timer.delay(0.5);
} else {
Catapult.launchRight.set(EXTENDED);
Timer.delay(3.0);
Catapult.launchRight.set(RETRACTED);
Timer.delay(2.0);
}
}
}
/**
* Enable/Disable character LED blink.
*
* @param blink True to blink.
*/
public void blink(boolean blink) {
byte[] buffer = new byte[1];
Util.consoleLog("%b", blink);
if (blink) buffer[0] = (byte) 0x83;
else buffer[0] = (byte) 0x81;
i2c.writeBulk(buffer);
Timer.delay(.01);
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
if (step == 0) {
Robot.canon.pushLaunchArm(pushSpeed);
System.out.print("Pushing for ");
System.out.println(pushTime);
Timer.delay(pushTime);
Robot.canon.returnLaunchArm(returnSpeed);
System.out.print("Pulling for ");
System.out.println(returnTime);
setTimeout(returnTime);
step = 1;
}
}
public synchronized void openCamera() {
if (m_id != -1) return; // Camera is already open
for (int i = 0; i < 3; i++) {
try {
m_id =
NIVision.IMAQdxOpenCamera(
m_name, NIVision.IMAQdxCameraControlMode.CameraControlModeController);
} catch (VisionException e) {
if (i == 2) throw e;
delay(2.0);
continue;
}
break;
}
}
// Called just before this Command runs the first time
protected void initialize() {
shooter.triggerRunning = true;
System.out.println("Trigger Forward");
shooter.setTrigger(-1);
Timer.delay(runTime);
System.out.println("Trigger Backward");
shooter.setTrigger(1);
Timer.delay(runTime);
System.out.println("Trigger Finished");
shooter.setTrigger(0);
shooter.triggerRunning = false;
//
// while (shooter.triggerPot.getAverageValue() < Shooter.TRIGGER_END) {
// shooter.setTrigger(-1);
// System.out.println(shooter.triggerPot.getAverageValue());
// }
// shooter.setTrigger(0);
// while (shooter.triggerPot.getAverageValue() > Shooter.TRIGGER_START) {
// shooter.setTrigger(.7);
//
// System.out.println(shooter.triggerPot.getAverageValue());
// }
// shooter.setTrigger(0);
}
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 teleopPeriodic() {
Definitions.drivetrain.arcadeDrive(
Definitions.xbox1.getRawAxis(1), Definitions.xbox1.getRawAxis(4), false);
DriveShifter.checkGearShift();
SparkyIntakeBar.loadingRoutine();
Shooter.firingRoutine(5000);
ArduinoComm.communicate();
try {
if (!backfailed && !frontfailed) {
if (Definitions.buttonbox.getRawButton(2) && !lastbutton4) {
if (currsession == front) {
NIVision.IMAQdxStopAcquisition(currsession);
currsession = back;
NIVision.IMAQdxConfigureGrab(currsession);
} else if (currsession == back) {
NIVision.IMAQdxStopAcquisition(currsession);
currsession = front;
NIVision.IMAQdxConfigureGrab(currsession);
}
}
}
lastbutton4 = Definitions.buttonbox.getRawButton(2);
NIVision.IMAQdxGrab(currsession, frame, 1);
CameraServer.getInstance().setImage(frame);
} catch (Exception e) {
// System.out.println("Camera problem");
}
SmartDashboard.putNumber("Pressure", 250 * (Definitions.pressuretrans.getVoltage() / 5.0) - 25);
SmartDashboard.putNumber("ballSeater", Definitions.ballholder.get() ? 1 : 0);
SmartDashboard.putBoolean(
"catapultReady", (250 * (Definitions.pressuretrans.getVoltage() / 5.0) - 25) > 40);
if (Definitions.joystick.getRawButton(3) && !lastbutton3) {
Definitions.flashlightrelay.set(
Definitions.flashlightrelay.get() == Relay.Value.kOff
? Relay.Value.kForward
: Relay.Value.kOff);
}
// System.out.println(Definitions.flashlightrelay.get());
lastbutton3 = Definitions.joystick.getRawButton(3);
System.out.println(Definitions.pdp.getCurrent(4));
Timer.delay(0.01);
}
/** This function is called once each time the robot enters operator control. */
public void operatorControl() {
drivetrain.setSafetyEnabled(true);
compressor.start();
while (isOperatorControl() && isEnabled()) {
// The throttle goes from 1 to -1, bottom to top. This
// scales it to go 0 to 1, bottom to top.
double throttle = (-leftStick.getRawAxis(3) + 1) / 2;
double drive = deadband(leftStick.getY()) * throttle;
double steer = deadband(-rightStick.getX()) * throttle;
drivetrain.arcadeDrive(drive, steer);
boolean shouldSpinWheels = leftStick.getRawButton(1);
boolean shouldFire = shouldSpinWheels && rightStick.getRawButton(1);
shooterWheels.set(shouldSpinWheels ? -1 : 0);
shooter.set(shouldFire);
Timer.delay(0.005); // wait for a motor update time
}
}
public void upLittle() {
robot.elevator.elevatorVics.set(1);
Timer.delay(0.2);
robot.elevator.elevatorVics.set(0);
}
public void mecanumDrive(double x, double y, double twist, double gyro) {
drive.mecanumDrive_Cartesian(x, y, twist, gyro);
Timer.delay(0.001);
}
public void operatorControl() {
log("Teleop Called Once!");
resetError(1);
resetError(2);
this.elevator.resetState();
// try{
showUserMessages("TeleOp");
this.arm.state = 0;
while (isOperatorControl() && isEnabled()) {
// robotdrive.setSafetyEnabled(true); //In case the program were to stop, this stops the
// motors from continuing to run.
if (!Util.checkButton(this, Constants.robotdrive_break_button)) {
// robotdrive.tankDrive(-doRobotdriveScaling(joystick_left.getY()),
// -doRobotdriveScaling(joystick_right.getY()));
robotdrive.arcadeDrive(
doRobotdriveScaling(joystick_left.getY()), doRobotdriveScaling(joystick_left.getX()));
}
// x, y int := 0, 1;
// if (driverstation.getDigitalIn(3)){
// this.test_CIM_1.set(driverstation.getAnalogIn(2));
// }else{
// this.test_CIM_1.set(0-driverstation.getAnalogIn(2));
// }
//
// if (driverstation.getDigitalIn(4)){
// this.test_CIM_2.set(driverstation.getAnalogIn(3));
// }else{
// this.test_CIM_2.set(0-driverstation.getAnalogIn(3));
// }
updateSmartDashboard();
this.shooter.updateStates(this);
this.shooterMotor.set(this.shooter.calculateShooterSpeed());
if (this.driverstation.getDigitalIn(4)) {
this.armMotor.set(this.driverstation.getAnalogIn(3));
} else {
this.armMotor.set(-this.driverstation.getAnalogIn(3));
}
this.arm.update();
if (this.arm.getArmSpeed() != 0D) {
this.armMotor.set(this.arm.getArmSpeed());
}
if (this.integ.doElevatorUpdate()) {
this.elevator.update();
this.Elevator_motor.set(this.elevator.getElevatorSpeed());
}
this.intake.update();
this.Intake_motor.set(this.intake.calculateIntakeSpeed());
if (this.intake.state == 1) {
this.Elevator_intake_motor.set(0.6D);
} else {
this.Elevator_intake_motor.set(0D);
}
if (this.driverstation.getDigitalIn(3)) {
this.pullDownJaguar.set(this.driverstation.getAnalogIn(2));
} else {
this.pullDownJaguar.set(-this.driverstation.getAnalogIn(2));
}
this.integ.update();
SmartDashboardUpdater.updateSmartDashboard(this);
if (Util.checkButton(this, 9)) {
this.elevator.numDiscs = 0;
}
if (Util.checkButton(this, 8)) {
this.elevator.state = 0;
this.integ.state = 0;
this.intake.state = 0;
this.integ.elevator_out_roller.set(0d);
this.arm.state = 0;
}
{
try {
SmartDashboard.putString("DEBUG*isConnected", "" + this.server.isConnected());
SmartDashboard.putString("DEBUG*isServer", "" + this.server.isServer());
SmartDashboard.putDouble("FPS~!", this.server.getNumber("FPS", -1d));
SmartDashboard.putDouble("NewDistance~!", this.server.getNumber("NewDistance", -1d));
// SmartDashboard.putString("NetworkTable Keys", this.server.);
log("W0RK1NG!");
} catch (Exception be) {
error("ERROR UPDATING SmartDashboard dtt STACK TRACE: " + be.getMessage(), 1);
}
}
Timer.delay(0.005); // and make sure we dont overload the cRIO
}
// }catch (Exception be){
// error("ERROR IN TELEOP! STACKTRACE: \n "+be.getMessage(), 2);
// }
}
@Override
public void execute() {
System.out.println("Starting execution");
try {
if (distance > 0) {
while (DriverStation.getInstance().isEnabled()
&& TKOHardware.getDriveTalon(0).getSetpoint() < distance) {
TKOHardware.getDriveTalon(0)
.set(TKOHardware.getDriveTalon(0).getSetpoint() + incrementer);
TKOHardware.getDriveTalon(2)
.set(TKOHardware.getDriveTalon(2).getSetpoint() + incrementer);
System.out.println(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
TKOLogger.getInstance()
.addMessage(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
Timer.delay(0.001);
}
} else {
while (DriverStation.getInstance().isEnabled()
&& TKOHardware.getDriveTalon(0).getSetpoint() > distance) {
TKOHardware.getDriveTalon(0)
.set(TKOHardware.getDriveTalon(0).getSetpoint() - incrementer);
TKOHardware.getDriveTalon(2)
.set(TKOHardware.getDriveTalon(2).getSetpoint() - incrementer);
System.out.println(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
TKOLogger.getInstance()
.addMessage(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
Timer.delay(0.001);
}
}
TKOHardware.getDriveTalon(0).set(distance);
TKOHardware.getDriveTalon(2).set(distance);
while (Math.abs(TKOHardware.getLeftDrive().getPosition() - distance) > threshold
&& DriverStation.getInstance().isEnabled()) {
// not close enough doe; actually gets stuck here
TKOLogger.getInstance()
.addMessage(
"NOT CLOSE ENOUGH TO TARGET DIST: "
+ (TKOHardware.getLeftDrive().getPosition() - distance));
Timer.delay(0.001);
}
} catch (TKOException e1) {
e1.printStackTrace();
System.out.println("Error at another expected spot, I would assume....");
}
System.out.println("Done executing");
}
/**
* This autonomous (along with the chooser code above) shows how to select between different
* autonomous modes using the dashboard. The sendable chooser code works with the Java
* SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
* uncomment the getString line to get the auto name from the text box below the Gyro
*
* <p>You can add additional auto modes by adding additional comparisons to the switch structure
* below with additional strings. If using the SendableChooser make sure to add them to the
* chooser code above as well.
*/
public void autonomousInit() {
autoSelected = (String) chooser.getSelected();
// autoSelected = SmartDashboard.getString("Auto Selector", defaultAuto);
System.out.println("Auto selected: " + autoSelected);
switch (autoSelected) {
case touch:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.53, 0.60); // Move forward - run motors at 60% speed for 2 sec
Timer.delay(2);
tankDrive.auto(0, 0); // Stop robot
break;
case lowBarCross:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.0, 0.60); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(.1);
tankDrive.auto(0.53, 0.60); // Move forward - run motors at 60% speed for 4.5 sec
Timer.delay(4.5);
tankDrive.auto(0, 0); // Stop robot
break;
case rockWallCross:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.0, 0.85); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(.1);
tankDrive.auto(0.78, 0.85); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(4.5);
tankDrive.auto(0, 0); // Stop robot
break;
case roughTerrainCross:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.0, 0.80); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(.1);
tankDrive.auto(0.73, 0.80); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(4);
tankDrive.auto(0, 0); // Stop robot
break;
default:
// Nothing ...
break;
}
}
public void run() {
stop = false;
boolean stream_response_received = false;
double last_valid_packet_time = 0.0;
int partial_binary_packet_count = 0;
int stream_response_receive_count = 0;
int timeout_count = 0;
int discarded_bytes_count = 0;
int port_reset_count = 0;
double last_stream_command_sent_timestamp = 0.0;
int updates_in_last_second = 0;
double last_second_start_time = 0;
try {
serial_port.setReadBufferSize(512);
serial_port.setTimeout(1.0);
serial_port.enableTermination('\n');
serial_port.flush();
serial_port.reset();
} catch (RuntimeException ex) {
ex.printStackTrace();
}
IMUProtocol.StreamResponse response = new IMUProtocol.StreamResponse();
byte[] stream_command = new byte[256];
int cmd_packet_length =
IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
try {
serial_port.reset();
serial_port.write(stream_command, cmd_packet_length);
serial_port.flush();
port_reset_count++;
// SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
} catch (RuntimeException ex) {
ex.printStackTrace();
}
while (!stop) {
try {
// Wait, with delays to conserve CPU resources, until
// bytes have arrived.
while (!stop && (serial_port.getBytesReceived() < 1)) {
Timer.delay(1.0 / update_rate_hz);
}
int packets_received = 0;
byte[] received_data = serial_port.read(256);
int bytes_read = received_data.length;
if (bytes_read > 0) {
byte_count += bytes_read;
int i = 0;
// Scan the buffer looking for valid packets
while (i < bytes_read) {
// Attempt to decode a packet
int bytes_remaining = bytes_read - i;
if (received_data[i] != IMUProtocol.PACKET_START_CHAR) {
/* Skip over received bytes until a packet start is detected. */
i++;
discarded_bytes_count++;
// SmartDashboard.putNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
continue;
} else {
if ((bytes_remaining > 2) && (received_data[i + 1] == (byte) '#')) {
/* Binary packet received; next byte is packet length-2 */
byte total_expected_binary_data_bytes = received_data[i + 2];
total_expected_binary_data_bytes += 2;
while (bytes_remaining < total_expected_binary_data_bytes) {
/* This binary packet contains an embedded */
/* end-of-line character. Continue to receive */
/* more data until entire packet is received. */
byte[] additional_received_data = serial_port.read(256);
byte_count += additional_received_data.length;
bytes_remaining += additional_received_data.length;
/* Resize array to hold existing and new data */
byte[] c = new byte[received_data.length + additional_received_data.length];
if (c.length > 0) {
System.arraycopy(received_data, 0, c, 0, received_data.length);
System.arraycopy(
additional_received_data,
0,
c,
received_data.length,
additional_received_data.length);
received_data = c;
} else {
/* Timeout waiting for remainder of binary packet */
i++;
bytes_remaining--;
partial_binary_packet_count++;
// SmartDashboard.putNumber("nav6 Partial Binary Packets",
// (double)partial_binary_packet_count);
continue;
}
}
}
}
int packet_length = decodePacketHandler(received_data, i, bytes_remaining);
if (packet_length > 0) {
packets_received++;
update_count++;
last_valid_packet_time = Timer.getFPGATimestamp();
updates_in_last_second++;
if ((last_valid_packet_time - last_second_start_time) > 1.0) {
// SmartDashboard.putNumber("nav6 UpdatesPerSec", (double)updates_in_last_second);
updates_in_last_second = 0;
last_second_start_time = last_valid_packet_time;
}
i += packet_length;
} else {
packet_length =
IMUProtocol.decodeStreamResponse(received_data, i, bytes_remaining, response);
if (packet_length > 0) {
packets_received++;
setStreamResponse(response);
stream_response_received = true;
i += packet_length;
stream_response_receive_count++;
// SmartDashboard.putNumber("nav6 Stream Responses",
// (double)stream_response_receive_count);
} else {
// current index is not the start of a valid packet; increment
i++;
}
}
}
if ((packets_received == 0) && (bytes_read == 256)) {
// Workaround for issue found in SerialPort implementation:
// No packets received and 256 bytes received; this
// condition occurs in the SerialPort. In this case,
// reset the serial port.
serial_port.reset();
port_reset_count++;
// SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
}
// If a stream configuration response has not been received within three seconds
// of operation, (re)send a stream configuration request
if (!stream_response_received
&& ((Timer.getFPGATimestamp() - last_stream_command_sent_timestamp) > 3.0)) {
cmd_packet_length =
IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
try {
last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
serial_port.write(stream_command, cmd_packet_length);
serial_port.flush();
} catch (RuntimeException ex2) {
ex2.printStackTrace();
}
} else {
// If no bytes remain in the buffer, and not awaiting a response, sleep a bit
if (stream_response_received && (serial_port.getBytesReceived() == 0)) {
Timer.delay(1.0 / update_rate_hz);
}
}
/* If receiving data, but no valid packets have been received in the last second */
/* the navX MXP may have been reset, but no exception has been detected. */
/* In this case , trigger transmission of a new stream_command, to ensure the */
/* streaming packet type is configured correctly. */
if ((Timer.getFPGATimestamp() - last_valid_packet_time) > 1.0) {
stream_response_received = false;
}
}
} catch (RuntimeException ex) {
// This exception typically indicates a Timeout
stream_response_received = false;
timeout_count++;
// SmartDashboard.putNumber("nav6 Serial Port Timeouts", (double)timeout_count);
// SmartDashboard.putString("LastNavExceptionBacktrace",ex.getStackTrace().toString());
// SmartDashboard.putString("LastNavException", ex.getMessage() + "; " + ex.toString());
}
}
}
