[Lesafian]

We're having a major issue with our robot. We're constantly losing communications. It seems to mainly happen after going over an obstacle, but it also can happen from just turning too sharp.

I was getting an error spammed, it was something like RobotDrive... Output not updated enough. I added drive.setSafetyEnabled(false); to the teleop code to get rid of that.

There are no other errors besides that.

I've re imaged the radio a few times, but I have not re imaged the roborio. The battery is also fully charged.

I'm wondering if it's a programming issue, here is my code. Going to a scrimmage tomorrow so a quick fix Is really needed.

package org.usfirst.frc.team6077.robot;

import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.vision.USBCamera;
	
public class Robot extends SampleRobot {

    // Declarations
	
	RobotDrive drive;
	
	CameraServer camserver;
	
	Compressor compressor;
	
    CameraServer server;
    USBCamera camera1; //camera2;
    
	DoubleSolenoid claw, pusher;
	
	DigitalInput limit1;
	Counter counter;
	
	Joystick xboxController;
	Joystick logitech3dpro;
	
	VictorSP drivemotor1;
	VictorSP drivemotor2;
	VictorSP drivemotor3;
	VictorSP drivemotor4;
	VictorSP slideMotor;
	VictorSP armMotor;

	// Autonomous Names
	final String defaultAuto = "Default Autonomous";
	final String customAuto = "Autonomous Choice 1";
	final String customAuto2 = "Autonomous Choice 2";
	final String customAuto3 = "Autonomous Choice 3";
	final String customAuto4 = "Autonomous Choice 4";
	final String customAuto5 = "Autonomous Choice 5";
	final String limitswitchpress = "Limit Switch Pressed";

	// Chooser
	SendableChooser chooser;

	public Robot() {
		//Change values later
		drivemotor1 = new VictorSP(0);
		drivemotor2 = new VictorSP(1);
		drivemotor3 = new VictorSP(2);
		drivemotor4 = new VictorSP(3);
		
		compressor = new Compressor(0);
		
        server = CameraServer.getInstance();
        camera1 = new USBCamera("cam0");
        //camera2 = new USBCamera("cam1");
        
		drive = new RobotDrive(drivemotor1, drivemotor2,
				drivemotor3, drivemotor4);
		drive.setExpiration(0.1);
		
		xboxController = new Joystick(0);
		logitech3dpro = new Joystick(1);
		
		armMotor = new VictorSP(5);
		slideMotor = new VictorSP(4);

		claw = new DoubleSolenoid(0, 1);
		pusher = new DoubleSolenoid(2, 3);
		
		limit1 = new DigitalInput(0);
		counter = new Counter(limit1);

		chooser = new SendableChooser();

	}

	public void robotInit() {
		compressor.setClosedLoopControl(true);
		compressor.start();
		chooser.addDefault("Default Autonomous", defaultAuto);
		chooser.addObject("Autonomous Choice 1", customAuto);
		chooser.addObject("Autonomous Choice 2", customAuto2);
		chooser.addObject("Autonomous Choice 3", customAuto3);
		chooser.addObject("Autonomous Choice 4", customAuto4);
		chooser.addObject("Autonomous Choice 5", customAuto5);
		SmartDashboard.putData("Autonomous Chooser", chooser);
	}

	public void autonomous() {
		drive.setSafetyEnabled(false);
		String autoSelected = (String) chooser.getSelected();
		System.out.println("Autonomous Mode selected: " + autoSelected);

		switch (autoSelected) {

		case customAuto5:		
			break;
		case customAuto4:		
			break;
		case customAuto3:
			break;
		case customAuto2:
			break;
		case customAuto:
			break;
		case defaultAuto:
		default:
			drive.drive(0.6, 0);
			Timer.delay(2.2);
			drive.drive(0, 0);
			break;
		}
		Scheduler.getInstance().run();
	}

	public void operatorControl() {
		drive.setSafetyEnabled(false);
		while (isOperatorControl() && isEnabled()) {
			
	        //Default Camera Selection
			camera1.setFPS(30);
	        server.setQuality(50);
	        server.startAutomaticCapture(camera1);
			
			// Move robot using left and right joystick
			
			//drive.tankDrive(xboxController.getRawAxis(1), xboxController.getRawAxis(5));
			drive.arcadeDrive(xboxController.getRawAxis(1), -xboxController.getRawAxis(0));

			//Xbox Controller
			
			//Arm Motor Test With Triggers
			if (Math.abs(xboxController.getRawAxis(3)) > .1) {
				
				armMotor.set(xboxController.getRawAxis(3));
				
			} else if (Math.abs(xboxController.getRawAxis(2)) > .1) {
				
				armMotor.set(-xboxController.getRawAxis(2));
				
			} else {
				
				armMotor.set(0);
				
			}
		
			// Move sliding mechanism forwards and backwards
			if (limit1.get() == true) {
				
				slideMotor.stopMotor();
				Timer.delay(0.25);
				slideMotor.set(-0.25);
				Timer.delay(0.5);
				
			} else if (limit1.get() == false) {
				
				if (Math.abs(xboxController.getRawAxis(5)) > .1) {
     
					slideMotor.set(-xboxController.getRawAxis(5));
		
				} else {
				
					slideMotor.set(0);
				}
			}
			
			
			//Logitech 3D PRO
			
			//Pusher Pneumatics System
			if (logitech3dpro.getRawButton(1)) {
				
				pusher.set(DoubleSolenoid.Value.kForward);
				claw.set(DoubleSolenoid.Value.kReverse);
				Timer.delay(1);
				pusher.set(DoubleSolenoid.Value.kReverse);
				
			}
			
			//Claw Pneumatics System
			if (logitech3dpro.getRawAxis(1) > .1) {
				
				claw.set(DoubleSolenoid.Value.kReverse);
				
			} else if (logitech3dpro.getRawAxis(1) < -.1) {
				
				claw.set(DoubleSolenoid.Value.kForward);
			}
			
			Timer.delay(0.005); //Wait for a motor update time
			
			//Camera Switcher
			/*if (logitech3dpro.getRawButton(3)) {
				
				camera2.setFPS(30);
				server.startAutomaticCapture(camera2);
				
			} else if (logitech3dpro.getRawButton(4)) {
				
				camera1.setFPS(30);
				server.startAutomaticCapture(camera1);
			*/
			}
		}
	public void test() {
	}
}