Robot Will Not Enable After Being Disabled

Technical Java
1

Hello CD,

Shortly before bag and tag, my team noticed that when practicing with our robot that if we disabled our robot, usually to tweak some hardware, we would need to reboot our cRio before we could enable it again. This problem appears to happen only with our code, and it does not happen to some modified sample code from http://wpilib.screenstepslive.com/s/3120/m/7885/l/79459-the-hello-world-of-frc-robot-programming.
I would appreciate any advice, insight or ideas to help us improve this situation.

Here is our code:

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package com.swatbotsrobotics.FRC.Core;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class SWATBots extends IterativeRobot {

    Joystick Driver = new Joystick(1); //Initialize two joystick/gamepad controllers 
    Joystick Gunner = new Joystick(2);
    Talon leftDrive = new Talon(1); //Initialize talons for drivetrain
    Talon rightDrive = new Talon(2);
    RobotDrive Chassis = new RobotDrive(leftDrive, rightDrive);
    Victor tiltMotors = new Victor(3);
    Victor punchMotors = new Victor(4);
    Victor BallPickup = new Victor(5);
    //Cannon_Tilt Tilt = new Cannon_Tilt();
    // LinearPunch Punch = new LinearPunch(); 
    DoubleSolenoid ShootSolenoid = new DoubleSolenoid(1, 2);
    Encoder tiltEncoder = new Encoder(1, 2, false, EncodingType.k4X);
    Encoder punchEncoder = new Encoder(4, 5);
    Timer punchTimer = new Timer();
    double tiltPosition, motorPower = 0, stickPosition, stickPower, stickSpeed = 0, speedSetpoint = 0, pidTiltpower = 0, stickOffset = 0;
    //miscellaneous variables 
    double forward_power_limit, reverse_power_limit;
    PIDController pidPunch = new PIDController(0.003, 0.00001, 0.0, punchEncoder, punchMotors);
    PIDController pidTilt = new PIDController(0.004, 0.000001, 0.0, tiltEncoder, tiltMotors);
    PIDController pidTiltspeed = new PIDController(0.002, 0.0, 0.0, tiltEncoder, tiltMotors);
    boolean tiltHold = false, trimUp = false, trimDown = false;
    double driveMultiplier;
 Gyro gyro = new Gyro(1);

//Autonomous variables
    Timer autoTime = new Timer();
    
    public void robotInit() {

    }

    public void autonomousInit()
    {
        autoTime.start();
    }
    
     
    public void autonomousPeriodic() {
        if(autoTime.get() < 5)
        {
            Chassis.drive(0.3, 0.01);
        }
        else{
            Chassis.drive(0.0, 0.0);
        }
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopInit() {
//autoTime.stop();
         gyro.setPIDSourceParameter(PIDSource.PIDSourceParameter.kAngle);
         gyro.setSensitivity(0.007);
   
tiltEncoder.setDistancePerPulse(360);
        punchEncoder.start();
        tiltEncoder.start();
        tiltPosition = tiltEncoder.get();


        Pneumatics();

        tiltEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kDistance);
        pidTilt.setOutputRange(-0.25, 0.25);
        pidTilt.setAbsoluteTolerance(5);
        punchEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kDistance);
        pidPunch.setOutputRange(0, 1);
        punchTimer.reset();
        pidTilt.reset();
        gyro.reset();
        

    }

    public void teleopPeriodic() {

        SmartDashboard.putNumber("Tilt Encoder", tiltEncoder.get());
         SmartDashboard.putNumber("Tilt Encoder Raw", tiltEncoder.getRaw());
        SmartDashboard.putNumber("Tilt Setpoint:", pidTilt.getSetpoint());
        SmartDashboard.putNumber("Tilt Motor Power:", tiltMotors.get());
        SmartDashboard.putNumber("Punch Encoder", punchEncoder.get());
        SmartDashboard.putNumber("Punch Motor Power:", punchMotors.get());
        SmartDashboard.putNumber("Tilt Encoder Rate", tiltEncoder.getRate());
                SmartDashboard.putNumber("Tilt PID output", pidTilt.get());
SmartDashboard.putBoolean("Tilt PID Enabled", pidTilt.isEnable());
SmartDashboard.putNumber("Gunner Stick Position", Gunner.getRawAxis(2));
SmartDashboard.putNumber("Driver Turn Power", Driver.getRawAxis(3) * -      1);
SmartDashboard.putNumber("Gyro", gyro.getAngle());
//Simple Halo Drive   
if(Driver.getRawButton(8))//Button 8 is the right bottom trigger on the logitech gamepads
{
    //If button 8 is pressed go into slow mode
    driveMultiplier = -0.75;
}
else{
    //Otherwise go full power
    driveMultiplier = -1.0;
}
        Chassis.arcadeDrive(Driver.getRawAxis(2) * driveMultiplier, Driver.getRawAxis(3) * driveMultiplier, true);

//START Cannon Tilt Code 
        stickPosition = -Gunner.getRawAxis(2);
        
        stickPower = pidTiltpower+stickPosition*0.5;
        
        if(stickPower > 0.3)
        {
            stickPower = 0.3;
        }
        else if(stickPower < -0.3)
        {
            stickPower = -0.3;
        }
        motorPower = tiltMotors.get();

        if (Gunner.getRawButton(8)) {
            tiltEncoder.reset();
            gyro.reset();
            
            tiltPosition = 0;
        }

        if(Gunner.getRawButton(2) || Gunner.getRawButton(3))
        {
            tiltHold = true;
        }
        else if(stickPosition > -0.05 && stickPosition < 0.05)
        {
            tiltHold = false;
        }
        //Control tilt upwards
        if (stickPosition < -0.05 && !tiltHold) {
            pidTilt.reset();
            tiltMotors.set(stickPower);
            tiltPosition = tiltEncoder.get();
          //  pidTiltpower=0;
        } //Control tilt downwards
        else if (stickPosition > 0.05 && !tiltHold){
            pidTilt.reset();
            tiltMotors.set(stickPower);
            tiltPosition = tiltEncoder.get();
           // pidTiltpower=0;
        } //Hold current position
        else {
            //Use PID to hold the position
            if(Gunner.getRawButton(2) && !trimUp)
            {
                tiltPosition -= 10;
            }
            else if(Gunner.getRawButton(3) && !trimDown)
            {
                tiltPosition += 10;
            }
        //   pidTilt.enable();
            pidTilt.setSetpoint(tiltPosition);
            stickOffset = -stickPosition;
            pidTiltpower = tiltMotors.get();
        }
trimUp = Gunner.getRawButton(2);
trimDown = Gunner.getRawButton(3);

//END Cannon Tilt code 


//START Linear Punch Code 
        //Shoot the Cannon!
        if (Gunner.getRawButton(1)) {
            ShootSolenoid.set(DoubleSolenoid.Value.kForward);
            punchEncoder.reset();
            pidPunch.reset();
        } else {
            ShootSolenoid.set(DoubleSolenoid.Value.kReverse);
        } //end Shoot the Cannon!

        //Reset the encoder if we reset too soon when we shoot
        if (punchEncoder.get() < 0) {
            punchEncoder.reset();
        }

        //Simple Pull back switch 
        if ( Gunner.getRawButton(10)) {
            pidPunch.reset();
            //If power is too high for punch power to other systems is reduced.
            punchMotors.set(0.8);
            punchTimer.start();
        } else if (Gunner.getRawButton(7)) {
            pidPunch.setSetpoint(2100);
            pidPunch.enable();
            punchTimer.start();
        } else if (Gunner.getRawButton(6) || punchTimer.get()> 5000) {
            pidPunch.setSetpoint(1100);
            pidPunch.enable();
            punchTimer.reset();
            punchTimer.stop();
        }


//END Linear Punch Code 



        //START Ball Pickup Code
        // BallPickup(); 

        if (Gunner.getRawButton(4)) {
            BallPickup.set(-0.35);
        } else if (Gunner.getRawButton(5)) {
            BallPickup.set(0.35);
        } else {
            BallPickup.set(0.0);
        }
//END Ball Pickup Code
        Timer.delay(0.02);
    }//END of Teleop Period 

    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
    }

    public void Pneumatics() //to initialize compressor + solenoids 
    {
        //First argument is digital input port pressure switch is connected to, 
        //second arg is relay port for Spike 
        Compressor airCompressor = new Compressor(6, 1);

        airCompressor.start();
    }

    public double pidRamp(double target, double setpoint, double delta) {
        double _setpoint, _HI_Band, _LO_Band;
        _setpoint = setpoint;
        _HI_Band = target + delta;
        _LO_Band = target - delta;

        if (_setpoint < _HI_Band && _setpoint > _LO_Band) {
            _setpoint = target;
        } else if (_setpoint < target) {
            _setpoint = _setpoint + delta;
        } else if (_setpoint > target) {
            _setpoint = _setpoint - delta;
        }

        return _setpoint;
    }

    public double speedLimit(double stick_power) {
        double speed_limit = 200;
        double speed = tiltEncoder.getRate();
        double motor_power = tiltMotors.get();
        if (speed < -speed_limit - 100) {
            forward_power_limit = 1;
            reverse_power_limit = reverse_power_limit + 0.03;
        } else if (speed < -speed_limit) {
            forward_power_limit = 1;
            reverse_power_limit = reverse_power_limit + 0.01;
        } else if (speed > speed_limit + 100) {
            forward_power_limit = forward_power_limit - 0.03;
            reverse_power_limit = -1;
        } else if (speed > speed_limit) {
            forward_power_limit = forward_power_limit - 0.01;
            reverse_power_limit = -1;
        } else {
            forward_power_limit = motor_power + 0.02;
            reverse_power_limit = motor_power - 0.02;
        }



        if (stick_power > forward_power_limit) {
            return forward_power_limit;
        } else if (stick_power < reverse_power_limit) {
            return reverse_power_limit;
        } else {
            return stick_power;
        }
    }
}

Here is our sample code that we know to work:

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class IterativeTest extends IterativeRobot {

    Victor leftDrive = new Victor(1);
    Victor rightDrive = new Victor(2);
    RobotDrive chassis = new RobotDrive(leftDrive, rightDrive);
    Joystick firstjoy = new Joystick(1);
    JoystickButton leftstick = new JoystickButton(firstjoy, 1);
    JoystickButton rightstick = new JoystickButton(firstjoy, 2);
 
    // Encoder Right_Encoder = new Encoder(1, 2, true, EncodingType.k4X);
    int motorCount;
    

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    /*
    public void robotInit() {
        Right_Encoder.start();
    }
*/
    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        chassis.tankDrive(firstjoy.getRawAxis(2)/* - 0.440945*/,firstjoy.getRawAxis(4));
      //  motorCount = Right_Encoder.get();
      //  SmartDashboard.putNumber("Encoder Count", motorCount);
        Timer.delay(0.01);
    }

    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
    }
}

Thanks,
The SWAT Bots

2

Are you sure you hit disable and not the E-stop? If e-stopped, the robot has to be manually rebooted before the DS will work with it again.

3

In particular, if you are modifying code on the driver station laptop, you will likely e-stop the robot when you type a space (the e-stop hotkey).

4

Thanks for the responses. As far as I can remember, we were using disable not e-stop. Also, when we re-enable the robot it seems to be running, but it just does nothing.

5

It looks like you are trying to construct a instance of the Compressor object within your Pneumatics method:

public void Pneumatics() //to initialize compressor + solenoids 
    {
        //First argument is digital input port pressure switch is connected to, 
        //second arg is relay port for Spike 
        Compressor airCompressor = new Compressor(6, 1);

        airCompressor.start();
    }

Since, you call this method in teleopInit(), this probably results in a error the second time you try to run teleop after rebooting the robot. You should see a stack trace in your developer console when this happens.

Try changing the creation of the airCompressor to be similar to the way you have created your other components (like the Talons).


    private Compressor airCompressor = new Compressor(6, 1);

public void Pneumatics() //to initialize compressor + solenoids 
    {
        // Make sure compressor is running
        airCompressor.start();
    }

If you still get a stack trace, examine the output of the stack trace as it should point out line numbers in your code where things went south.

Hope that helps,
Paul

6

Thanks for all of the ideas, everyone. From what I can tell the suggestion from Paul appears to likely be what our problem is. I have changed my code accordingly and will test when possible.

Kyle